accel and gyro calibration refactor and cleanup

- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values 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) instead of TC scale
This commit is contained in:
Daniel Agar
2020-06-17 22:50:09 -04:00
committed by GitHub
parent 588d551098
commit f55ed0992c
99 changed files with 933 additions and 2229 deletions
+4
View File
@@ -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 "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 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 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 "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 "df"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
@@ -820,6 +822,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 "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 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 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 "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 "df -h"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'
Regular → Executable
+16 -36
View File
@@ -122,10 +122,7 @@ gyro_0_params = {
'TC_G0_X0_2':0.0, 'TC_G0_X0_2':0.0,
'TC_G0_X1_2':0.0, 'TC_G0_X1_2':0.0,
'TC_G0_X2_2':0.0, 'TC_G0_X2_2':0.0,
'TC_G0_X3_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
} }
# curve fit the data for gyro 0 corrections # curve fit the data for gyro 0 corrections
@@ -174,7 +171,7 @@ if num_gyros >= 1:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b') plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b')
plt.plot(temp_resample,gyro_0_x_resample,'r') plt.plot(temp_resample,gyro_0_x_resample,'r')
plt.title('Gyro 0 Bias vs Temperature') plt.title('Gyro 0 ({}) Bias vs Temperature'.format(gyro_0_params['TC_G0_ID']))
plt.ylabel('X bias (rad/s)') plt.ylabel('X bias (rad/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -218,10 +215,7 @@ gyro_1_params = {
'TC_G1_X0_2':0.0, 'TC_G1_X0_2':0.0,
'TC_G1_X1_2':0.0, 'TC_G1_X1_2':0.0,
'TC_G1_X2_2':0.0, 'TC_G1_X2_2':0.0,
'TC_G1_X3_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
} }
# curve fit the data for gyro 1 corrections # curve fit the data for gyro 1 corrections
@@ -270,7 +264,7 @@ if num_gyros >= 2:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b') plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b')
plt.plot(temp_resample,gyro_1_x_resample,'r') plt.plot(temp_resample,gyro_1_x_resample,'r')
plt.title('Gyro 1 Bias vs Temperature') plt.title('Gyro 1 ({}) Bias vs Temperature'.format(gyro_1_params['TC_G1_ID']))
plt.ylabel('X bias (rad/s)') plt.ylabel('X bias (rad/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -314,10 +308,7 @@ gyro_2_params = {
'TC_G2_X0_2':0.0, 'TC_G2_X0_2':0.0,
'TC_G2_X1_2':0.0, 'TC_G2_X1_2':0.0,
'TC_G2_X2_2':0.0, 'TC_G2_X2_2':0.0,
'TC_G2_X3_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
} }
# curve fit the data for gyro 2 corrections # curve fit the data for gyro 2 corrections
@@ -366,7 +357,7 @@ if num_gyros >= 3:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b') plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b')
plt.plot(temp_resample,gyro_2_x_resample,'r') plt.plot(temp_resample,gyro_2_x_resample,'r')
plt.title('Gyro 2 Bias vs Temperature') plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_2_params['TC_G2_ID']))
plt.ylabel('X bias (rad/s)') plt.ylabel('X bias (rad/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -410,10 +401,7 @@ accel_0_params = {
'TC_A0_X0_2':0.0, 'TC_A0_X0_2':0.0,
'TC_A0_X1_2':0.0, 'TC_A0_X1_2':0.0,
'TC_A0_X2_2':0.0, 'TC_A0_X2_2':0.0,
'TC_A0_X3_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
} }
# curve fit the data for accel 0 corrections # curve fit the data for accel 0 corrections
@@ -465,7 +453,7 @@ if num_accels >= 1:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_accel_0['temperature'],correction_x,'b') plt.plot(sensor_accel_0['temperature'],correction_x,'b')
plt.plot(temp_resample,correction_x_resample,'r') plt.plot(temp_resample,correction_x_resample,'r')
plt.title('Accel 0 Bias vs Temperature') plt.title('Accel 0 ({}) Bias vs Temperature'.format(accel_0_params['TC_A0_ID']))
plt.ylabel('X bias (m/s/s)') plt.ylabel('X bias (m/s/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -509,10 +497,7 @@ accel_1_params = {
'TC_A1_X0_2':0.0, 'TC_A1_X0_2':0.0,
'TC_A1_X1_2':0.0, 'TC_A1_X1_2':0.0,
'TC_A1_X2_2':0.0, 'TC_A1_X2_2':0.0,
'TC_A1_X3_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
} }
# curve fit the data for accel 1 corrections # curve fit the data for accel 1 corrections
@@ -564,7 +549,7 @@ if num_accels >= 2:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_accel_1['temperature'],correction_x,'b') plt.plot(sensor_accel_1['temperature'],correction_x,'b')
plt.plot(temp_resample,correction_x_resample,'r') plt.plot(temp_resample,correction_x_resample,'r')
plt.title('Accel 1 Bias vs Temperature') plt.title('Accel 1 ({}) Bias vs Temperature'.format(accel_1_params['TC_A1_ID']))
plt.ylabel('X bias (m/s/s)') plt.ylabel('X bias (m/s/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -608,10 +593,7 @@ accel_2_params = {
'TC_A2_X0_2':0.0, 'TC_A2_X0_2':0.0,
'TC_A2_X1_2':0.0, 'TC_A2_X1_2':0.0,
'TC_A2_X2_2':0.0, 'TC_A2_X2_2':0.0,
'TC_A2_X3_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
} }
# curve fit the data for accel 2 corrections # curve fit the data for accel 2 corrections
@@ -663,7 +645,7 @@ if num_accels >= 3:
plt.subplot(3,1,1) plt.subplot(3,1,1)
plt.plot(sensor_accel_2['temperature'],correction_x,'b') plt.plot(sensor_accel_2['temperature'],correction_x,'b')
plt.plot(temp_resample,correction_x_resample,'r') plt.plot(temp_resample,correction_x_resample,'r')
plt.title('Accel 2 Bias vs Temperature') plt.title('Accel 2 ({}) Bias vs Temperature'.format(accel_2_params['TC_A2_ID']))
plt.ylabel('X bias (m/s/s)') plt.ylabel('X bias (m/s/s)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -701,8 +683,7 @@ baro_0_params = {
'TC_B0_X2':0.0, 'TC_B0_X2':0.0,
'TC_B0_X3':0.0, 'TC_B0_X3':0.0,
'TC_B0_X4':0.0, 'TC_B0_X4':0.0,
'TC_B0_X5':0.0, 'TC_B0_X5':0.0
'TC_B0_SCL':1.0,
} }
# curve fit the data for baro 0 corrections # curve fit the data for baro 0 corrections
@@ -734,7 +715,7 @@ plt.figure(7,figsize=(20,13))
# draw plots # draw plots
plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b') plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_0_x_resample,'r') plt.plot(temp_resample,baro_0_x_resample,'r')
plt.title('Baro 0 Bias vs Temperature') plt.title('Baro 0 ({}) Bias vs Temperature'.format(baro_0_params['TC_B0_ID']))
plt.ylabel('Z bias (Pa)') plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -753,7 +734,6 @@ baro_1_params = {
'TC_B1_X3':0.0, 'TC_B1_X3':0.0,
'TC_B1_X4':0.0, 'TC_B1_X4':0.0,
'TC_B1_X5':0.0, 'TC_B1_X5':0.0,
'TC_B1_SCL':1.0,
} }
if num_baros >= 2: if num_baros >= 2:
@@ -787,7 +767,7 @@ if num_baros >= 2:
# draw plots # draw plots
plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b') plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_1_x_resample,'r') plt.plot(temp_resample,baro_1_x_resample,'r')
plt.title('Baro 1 Bias vs Temperature') plt.title('Baro 1 ({}) Bias vs Temperature'.format(baro_1_params['TC_B1_ID']))
plt.ylabel('Z bias (Pa)') plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
@@ -840,7 +820,7 @@ if num_baros >= 3:
# draw plots # draw plots
plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b') plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_2_x_resample,'r') plt.plot(temp_resample,baro_2_x_resample,'r')
plt.title('Baro 2 Bias vs Temperature') plt.title('Baro 2 ({}) Bias vs Temperature'.format(baro_2_params['TC_B2_ID']))
plt.ylabel('Z bias (Pa)') plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)') plt.xlabel('temperature (degC)')
plt.grid() plt.grid()
+2
View File
@@ -11,3 +11,5 @@ uint8 samples # number of valid samples
int16[32] x # acceleration in the NED X board axis in m/s/s int16[32] x # acceleration in the NED X board axis in m/s/s
int16[32] y # acceleration in the NED Y board axis in m/s/s int16[32] y # acceleration in the NED Y board axis in m/s/s
int16[32] z # acceleration in the NED Z board axis in m/s/s int16[32] z # acceleration in the NED Z board axis in m/s/s
uint8 rotation # Direction the sensor faces (see Rotation enum)
+11 -42
View File
@@ -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 # 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 # 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 uint32[3] gyro_device_ids
float32[3] gyro_offset_0 # gyro XYZ offsets in the sensor frame in rad/s float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s
float32[3] gyro_scale_0 # gyro XYZ scale factors in the sensor frame 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 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
# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset # 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 # 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 uint32[3] accel_device_ids
float32[3] accel_offset_0 # accelerometer XYZ offsets in the sensor frame in m/s/s float32[3] accel_offset_0 # accelerometer 0 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_scale_0 # accelerometer XYZ scale factors in the sensor frame 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 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
# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset # 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 # 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 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
+2
View File
@@ -11,3 +11,5 @@ uint8 samples # number of valid samples
int16[32] x # angular velocity in the NED X board axis in rad/s int16[32] x # angular velocity in the NED X board axis in rad/s
int16[32] y # angular velocity in the NED Y board axis in rad/s int16[32] y # angular velocity in the NED Y board axis in rad/s
int16[32] z # angular velocity in the NED Z board axis in rad/s int16[32] z # angular velocity in the NED Z board axis in rad/s
uint8 rotation # Direction the sensor faces (see Rotation enum)
-2
View File
@@ -191,6 +191,4 @@ BMP280::print_status()
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_measure_perf); perf_print_counter(_measure_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_baro.print_status();
} }
-2
View File
@@ -105,8 +105,6 @@ BMP388::print_status()
perf_print_counter(_measure_perf); perf_print_counter(_measure_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
printf("measurement interval: %u us \n", _measure_interval); printf("measurement interval: %u us \n", _measure_interval);
_px4_baro.print_status();
} }
void void
-2
View File
@@ -284,8 +284,6 @@ DPS310::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_barometer.print_status();
} }
} // namespace dps310 } // namespace dps310
@@ -184,5 +184,4 @@ void LPS22HB::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_baro.print_status();
} }
-2
View File
@@ -199,6 +199,4 @@ void LPS25H::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_barometer.print_status();
} }
@@ -284,6 +284,4 @@ void MPL3115A2::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_barometer.print_status();
} }
-2
View File
@@ -340,8 +340,6 @@ void MS5611::print_status()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
printf("device: %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607"); printf("device: %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
_px4_barometer.print_status();
} }
namespace ms5611 namespace ms5611
@@ -327,8 +327,6 @@ CM8JL65::print_info()
{ {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
void void
@@ -267,8 +267,6 @@ LeddarOne::print_info()
{ {
perf_print_counter(_comms_error); perf_print_counter(_comms_error);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
_px4_rangefinder.print_status();
} }
void void
@@ -297,6 +297,4 @@ void SF0X::print_info()
{ {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
@@ -278,7 +278,6 @@ void SF1XX::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
void SF1XX::print_usage() void SF1XX::print_usage()
@@ -147,6 +147,4 @@ void SRF02::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
@@ -268,6 +268,4 @@ void TERARANGER::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
@@ -264,6 +264,4 @@ TFMINI::print_info()
printf("Using port '%s'\n", _port); printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
@@ -221,6 +221,4 @@ void AerotennaULanding::print_info()
{ {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
_px4_rangefinder.print_status();
} }
@@ -177,7 +177,6 @@ void VL53L0X::print_status()
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
_px4_rangefinder.print_status();
} }
int VL53L0X::probe() int VL53L0X::probe()
-75
View File
@@ -1,75 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_accel.h
*
* Accelerometer driver interface.
*/
#ifndef _DRV_ACCEL_H
#define _DRV_ACCEL_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define ACCEL_BASE_DEVICE_PATH "/dev/accel"
#include <uORB/topics/sensor_accel.h>
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
struct accel_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
* ioctl() definitions
*
* Accelerometer drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _ACCELIOCBASE (0x2100)
#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n))
/** set the accel scaling constants to the structure pointed to by (arg) */
#define ACCELIOCSSCALE _ACCELIOC(5)
#endif /* _DRV_ACCEL_H */
-70
View File
@@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_gyro.h
*
* Gyroscope driver interface.
*/
#ifndef _DRV_GYRO_H
#define _DRV_GYRO_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define GYRO_BASE_DEVICE_PATH "/dev/gyro"
#include <uORB/topics/sensor_gyro.h>
/** gyro scaling factors; Vout = Vin + Voffset */
struct gyro_calibration_s {
float x_offset;
float y_offset;
float z_offset;
};
/*
* ioctl() definitions
*/
#define _GYROIOCBASE (0x2300)
#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n))
/** set the gyro scaling constants to (arg) */
#define GYROIOCSSCALE _GYROIOC(4)
#endif /* _DRV_GYRO_H */
+1 -1
View File
@@ -85,4 +85,4 @@
* - save/serialise for saving tuned mixers. * - save/serialise for saving tuned mixers.
*/ */
#endif /* _DRV_ACCEL_H */ #endif /* _DRV_MIXER_H */
-3
View File
@@ -308,9 +308,6 @@ ADIS16448::print_status()
perf_print_counter(_perf_bad_transfer); perf_print_counter(_perf_bad_transfer);
perf_print_counter(_perf_crc_bad); perf_print_counter(_perf_crc_bad);
_px4_accel.print_status();
_px4_baro.print_status();
_px4_gyro.print_status();
_px4_mag.print_status(); _px4_mag.print_status();
} }
-2
View File
@@ -385,6 +385,4 @@ ADIS16477::print_status()
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers); perf_print_counter(_bad_transfers);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
-2
View File
@@ -505,6 +505,4 @@ ADIS16497::print_status()
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers); perf_print_counter(_bad_transfers);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
-1
View File
@@ -431,7 +431,6 @@ void BMA180::print_status()
{ {
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
_px4_accel.print_status();
} }
void BMA180::print_usage() void BMA180::print_usage()
-2
View File
@@ -599,6 +599,4 @@ void BMI160::print_status()
perf_print_counter(_good_transfers); perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries); perf_print_counter(_reset_retries);
perf_print_counter(_duplicates); perf_print_counter(_duplicates);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
@@ -82,8 +82,6 @@ void BMI055_Accelerometer::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
} }
int BMI055_Accelerometer::probe() int BMI055_Accelerometer::probe()
@@ -82,8 +82,6 @@ void BMI055_Gyroscope::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_gyro.print_status();
} }
int BMI055_Gyroscope::probe() int BMI055_Gyroscope::probe()
@@ -82,8 +82,6 @@ void BMI088_Accelerometer::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
} }
int BMI088_Accelerometer::probe() int BMI088_Accelerometer::probe()
@@ -82,8 +82,6 @@ void BMI088_Gyroscope::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_gyro.print_status();
} }
int BMI088_Gyroscope::probe() int BMI088_Gyroscope::probe()
@@ -418,7 +418,6 @@ void FXAS21002C::print_status()
} }
} }
_px4_gyro.print_status();
} }
void void
-2
View File
@@ -828,7 +828,5 @@ ICM20948::print_status()
perf_print_counter(_good_transfers); perf_print_counter(_good_transfers);
perf_print_counter(_duplicates); perf_print_counter(_duplicates);
_px4_accel.print_status();
_px4_gyro.print_status();
_mag.print_status(); _mag.print_status();
} }
@@ -102,8 +102,6 @@ void ICM20602::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM20602::probe() int ICM20602::probe()
@@ -107,8 +107,6 @@ void ICM20608G::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM20608G::probe() int ICM20608G::probe()
@@ -102,8 +102,6 @@ void ICM20649::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM20649::probe() int ICM20649::probe()
@@ -106,9 +106,6 @@ void ICM20689::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM20689::probe() int ICM20689::probe()
@@ -124,8 +124,6 @@ void ICM20948::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
if (_slave_ak09916_magnetometer) { if (_slave_ak09916_magnetometer) {
_slave_ak09916_magnetometer->PrintInfo(); _slave_ak09916_magnetometer->PrintInfo();
@@ -101,9 +101,6 @@ void ICM40609D::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM40609D::probe() int ICM40609D::probe()
@@ -102,8 +102,6 @@ void ICM42688P::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int ICM42688P::probe() int ICM42688P::probe()
@@ -107,8 +107,6 @@ void MPU6000::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int MPU6000::probe() int MPU6000::probe()
@@ -102,8 +102,6 @@ void MPU6500::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
int MPU6500::probe() int MPU6500::probe()
@@ -125,8 +125,6 @@ void MPU9250::print_status()
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
if (_slave_ak8963_magnetometer) { if (_slave_ak8963_magnetometer) {
_slave_ak8963_magnetometer->PrintInfo(); _slave_ak8963_magnetometer->PrintInfo();
-1
View File
@@ -414,7 +414,6 @@ L3GD20::print_status()
} }
} }
_px4_gyro.print_status();
} }
void void
-2
View File
@@ -883,8 +883,6 @@ MPU6000::print_status()
perf_print_counter(_reset_retries); perf_print_counter(_reset_retries);
perf_print_counter(_duplicates); perf_print_counter(_duplicates);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
void void
-2
View File
@@ -670,7 +670,5 @@ MPU9250::print_status()
perf_print_counter(_bad_registers); perf_print_counter(_bad_registers);
perf_print_counter(_duplicates); perf_print_counter(_duplicates);
_px4_accel.print_status();
_px4_gyro.print_status();
_mag.print_status(); _mag.print_status();
} }
@@ -339,6 +339,4 @@ void ISM330DLC::print_status()
perf_print_counter(_drdy_count_perf); perf_print_counter(_drdy_count_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }
-2
View File
@@ -309,6 +309,4 @@ void LSM9DS1::print_status()
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
_px4_accel.print_status();
_px4_gyro.print_status();
} }

Some files were not shown because too many files have changed in this diff Show More