Skip to content

Commit

Permalink
accel and gyro calibration refactor and cleanup
Browse files Browse the repository at this point in the history
 - remove all remaining IOCTLs for accel and gyro
 - move accel/gyro calibration entirely to sensors module
 - calibration no longer needs to clear existing before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
 - gyro and baro scale are completely unused
 - regular accel calibration scale can be used (CAL_ACC*_xSCALE)
  • Loading branch information
dagar committed May 10, 2020
1 parent 2bbdef2 commit 7bdb0ee
Show file tree
Hide file tree
Showing 50 changed files with 917 additions and 2,077 deletions.
4 changes: 4 additions & 0 deletions .ci/Jenkinsfile-hardware
Original file line number Diff line number Diff line change
Expand Up @@ -759,6 +759,8 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
Expand Down Expand Up @@ -821,6 +823,8 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "adc test"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'
Expand Down
34 changes: 7 additions & 27 deletions Tools/process_sensor_caldata.py
Original file line number Diff line number Diff line change
Expand Up @@ -124,10 +124,7 @@ def is_valid_directory(parser, arg):
'TC_G0_X0_2':0.0,
'TC_G0_X1_2':0.0,
'TC_G0_X2_2':0.0,
'TC_G0_X3_2':0.0,
'TC_G0_SCL_0':1.0,
'TC_G0_SCL_1':1.0,
'TC_G0_SCL_2':1.0
'TC_G0_X3_2':0.0
}

# curve fit the data for gyro 0 corrections
Expand Down Expand Up @@ -220,10 +217,7 @@ def is_valid_directory(parser, arg):
'TC_G1_X0_2':0.0,
'TC_G1_X1_2':0.0,
'TC_G1_X2_2':0.0,
'TC_G1_X3_2':0.0,
'TC_G1_SCL_0':1.0,
'TC_G1_SCL_1':1.0,
'TC_G1_SCL_2':1.0
'TC_G1_X3_2':0.0
}

# curve fit the data for gyro 1 corrections
Expand Down Expand Up @@ -316,10 +310,7 @@ def is_valid_directory(parser, arg):
'TC_G2_X0_2':0.0,
'TC_G2_X1_2':0.0,
'TC_G2_X2_2':0.0,
'TC_G2_X3_2':0.0,
'TC_G2_SCL_0':1.0,
'TC_G2_SCL_1':1.0,
'TC_G2_SCL_2':1.0
'TC_G2_X3_2':0.0
}

# curve fit the data for gyro 2 corrections
Expand Down Expand Up @@ -412,10 +403,7 @@ def is_valid_directory(parser, arg):
'TC_A0_X0_2':0.0,
'TC_A0_X1_2':0.0,
'TC_A0_X2_2':0.0,
'TC_A0_X3_2':0.0,
'TC_A0_SCL_0':1.0,
'TC_A0_SCL_1':1.0,
'TC_A0_SCL_2':1.0
'TC_A0_X3_2':0.0
}

# curve fit the data for accel 0 corrections
Expand Down Expand Up @@ -511,10 +499,7 @@ def is_valid_directory(parser, arg):
'TC_A1_X0_2':0.0,
'TC_A1_X1_2':0.0,
'TC_A1_X2_2':0.0,
'TC_A1_X3_2':0.0,
'TC_A1_SCL_0':1.0,
'TC_A1_SCL_1':1.0,
'TC_A1_SCL_2':1.0
'TC_A1_X3_2':0.0
}

# curve fit the data for accel 1 corrections
Expand Down Expand Up @@ -610,10 +595,7 @@ def is_valid_directory(parser, arg):
'TC_A2_X0_2':0.0,
'TC_A2_X1_2':0.0,
'TC_A2_X2_2':0.0,
'TC_A2_X3_2':0.0,
'TC_A2_SCL_0':1.0,
'TC_A2_SCL_1':1.0,
'TC_A2_SCL_2':1.0
'TC_A2_X3_2':0.0
}

# curve fit the data for accel 2 corrections
Expand Down Expand Up @@ -703,8 +685,7 @@ def is_valid_directory(parser, arg):
'TC_B0_X2':0.0,
'TC_B0_X3':0.0,
'TC_B0_X4':0.0,
'TC_B0_X5':0.0,
'TC_B0_SCL':1.0,
'TC_B0_X5':0.0
}

# curve fit the data for baro 0 corrections
Expand Down Expand Up @@ -755,7 +736,6 @@ def is_valid_directory(parser, arg):
'TC_B1_X3':0.0,
'TC_B1_X4':0.0,
'TC_B1_X5':0.0,
'TC_B1_SCL':1.0,
}

if num_baros >= 2:
Expand Down
53 changes: 11 additions & 42 deletions msg/sensor_correction.msg
Original file line number Diff line number Diff line change
Expand Up @@ -6,52 +6,21 @@ uint64 timestamp # time since system start (microseconds)

# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32[3] gyro_offset_0 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_0 # gyro XYZ scale factors in the sensor frame

# corrections for uORB instance 1
float32[3] gyro_offset_1 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_1 # gyro XYZ scale factors in the sensor frame

# corrections for uORB instance 2
float32[3] gyro_offset_2 # gyro XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_2 # gyro XYZ scale factors in the sensor frame
uint32[3] gyro_device_ids
float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s

# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32[3] accel_offset_0 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_0 # accelerometer XYZ scale factors in the sensor frame

# corrections for uORB instance 1
float32[3] accel_offset_1 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_1 # accelerometer XYZ scale factors in the sensor frame

# corrections for uORB instance 2
float32[3] accel_offset_2 # accelerometer XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_2 # accelerometer XYZ scale factors in the sensor frame
uint32[3] accel_device_ids
float32[3] accel_offset_0 # accelerometer 0 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_1 # accelerometer 1 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_2 # accelerometer 2 XYZ offsets in the sensor frame in m/s/s

# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
# corrections for uORB instance 0
float32 baro_offset_0 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_0 # barometric pressure scale factors in the sensor frame

# corrections for uORB instance 1
float32 baro_offset_1 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_1 # barometric pressure scale factors in the sensor frame

# corrections for uORB instance 2
float32 baro_offset_2 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_2 # barometric pressure scale factors in the sensor frame

# Mapping from uORB index to parameter index for each sensor type. For example accel_mapping[1] contains the
# compensation parameter system index for the sensor_accel uORB index 1 data.
uint8[3] gyro_mapping
uint8[3] accel_mapping
uint8[3] baro_mapping

uint32[3] gyro_device_ids
uint32[3] accel_device_ids
uint32[3] baro_device_ids
float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in m/s/s
float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in m/s/s
float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in m/s/s
75 changes: 0 additions & 75 deletions src/drivers/drv_accel.h

This file was deleted.

70 changes: 0 additions & 70 deletions src/drivers/drv_gyro.h

This file was deleted.

2 changes: 1 addition & 1 deletion src/drivers/drv_mixer.h
Original file line number Diff line number Diff line change
Expand Up @@ -85,4 +85,4 @@
* - save/serialise for saving tuned mixers.
*/

#endif /* _DRV_ACCEL_H */
#endif /* _DRV_MIXER_H */
2 changes: 0 additions & 2 deletions src/examples/matlab_csv_serial/matlab_csv_serial.c
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,6 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
Expand Down
Loading

0 comments on commit 7bdb0ee

Please sign in to comment.