diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index a50f5a0e94..c05c3b3191 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -866,6 +866,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_local_position"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_magnetometer"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' @@ -932,6 +933,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_local_position"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_magnetometer"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"' diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 762580caf9..797bd4c5c6 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -46,10 +46,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: raise PreconditionError('could not find innovation data') try: - sensor_preflight = ulog.get_dataset('sensor_preflight').data + sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data print('found sensor_preflight data') except: - raise PreconditionError('could not find sensor_preflight data') + raise PreconditionError('could not find sensor_preflight_imu data') control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) @@ -61,10 +61,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: with PdfPages(output_plot_filename) as pdf_pages: # plot IMU consistency data - if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and ( - 'gyro_inconsistency_rad_s' in sensor_preflight.keys()): + if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and ( + 'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()): data_plot = TimeSeriesPlot( - sensor_preflight, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']], + sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']], x_labels=['data index', 'data index'], y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'], plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 52cb19beaf..3451651961 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -119,7 +119,8 @@ set(msg_files sensor_gyro.msg sensor_gyro_fifo.msg sensor_mag.msg - sensor_preflight.msg + sensor_preflight_imu.msg + sensor_preflight_mag.msg sensor_selection.msg subsystem_info.msg system_power.msg diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg index 63e1385e70..5d002aae1b 100644 --- a/msg/sensor_mag.msg +++ b/msg/sensor_mag.msg @@ -11,4 +11,4 @@ float32 temperature # temperature in degrees celsius uint32 error_count -bool is_external # if true the mag is external (i.e. not built into the board) +bool is_external # if true the mag is external (i.e. not built into the board) diff --git a/msg/sensor_preflight.msg b/msg/sensor_preflight_imu.msg similarity index 80% rename from msg/sensor_preflight.msg rename to msg/sensor_preflight_imu.msg index 1c97183cf7..c166f62df0 100644 --- a/msg/sensor_preflight.msg +++ b/msg/sensor_preflight_imu.msg @@ -5,4 +5,3 @@ uint64 timestamp # time since system start (microseconds) float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). -float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/msg/sensor_preflight_mag.msg b/msg/sensor_preflight_mag.msg new file mode 100644 index 0000000000..2b5333c4b8 --- /dev/null +++ b/msg/sensor_preflight_mag.msg @@ -0,0 +1,7 @@ +# +# Pre-flight sensor check metrics. +# The topic will not be updated when the vehicle is armed +# +uint64 timestamp # time since system start (microseconds) + +float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/msg/sensor_selection.msg b/msg/sensor_selection.msg index 08e75122b4..799ccf18a3 100644 --- a/msg/sensor_selection.msg +++ b/msg/sensor_selection.msg @@ -5,4 +5,3 @@ uint64 timestamp # time since system start (microseconds) uint32 accel_device_id # unique device ID for the selected accelerometers uint32 gyro_device_id # unique device ID for the selected rate gyros -uint32 mag_device_id # unique device ID for the selected magnetometer diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 7be92d9d2a..c529fc895d 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -158,7 +158,7 @@ rtps: id: 67 - msg: sensor_mag id: 68 - - msg: sensor_preflight + - msg: sensor_preflight_imu id: 69 - msg: sensor_selection id: 70 @@ -297,6 +297,8 @@ rtps: id: 132 - msg: telemetry_heartbeat id: 133 + - msg: sensor_preflight_mag + id: 134 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_magnetometer.msg b/msg/vehicle_magnetometer.msg index bb0146c724..e279d16bba 100644 --- a/msg/vehicle_magnetometer.msg +++ b/msg/vehicle_magnetometer.msg @@ -1,3 +1,8 @@ -uint64 timestamp # time since system start (microseconds) -float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the selected magnetometer + +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e7e04d5d31..ae4ad20999 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -51,30 +51,4 @@ #include -/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ -struct mag_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; -}; - -/* - * ioctl() definitions - */ - -#define _MAGIOCBASE (0x2400) -#define _MAGIOC(_n) (_PX4_IOC(_MAGIOCBASE, _n)) - -/** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(4) - -/** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(5) - -/** determine if mag is external or onboard */ -#define MAGIOCGEXTERNAL _MAGIOC(11) - #endif /* _DRV_MAG_H */ diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 3c88d3e8d8..a0b3afc6f9 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -307,8 +307,6 @@ ADIS16448::print_status() perf_print_counter(_perf_transfer); perf_print_counter(_perf_bad_transfer); perf_print_counter(_perf_crc_bad); - - _px4_mag.print_status(); } void diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 0c7ed50ab8..13f97d45b1 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -75,8 +75,6 @@ void ICM20948_AK09916::PrintInfo() { perf_print_counter(_bad_transfer_perf); perf_print_counter(_magnetic_sensor_overflow_perf); - - _px4_mag.print_status(); } void ICM20948_AK09916::Run() diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index ee5a90e848..5bf62d4ebc 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -75,8 +75,6 @@ void MPU9250_AK8963::PrintInfo() { perf_print_counter(_bad_transfer_perf); perf_print_counter(_magnetic_sensor_overflow_perf); - - _px4_mag.print_status(); } void MPU9250_AK8963::Run() diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h index 916a6fd82d..2e601cf166 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.h +++ b/src/drivers/imu/mpu9250/MPU9250_mag.h @@ -124,8 +124,6 @@ public: bool ak8963_check_id(uint8_t &id); bool ak8963_read_adjustments(); - void print_status() { _px4_mag.print_status(); } - protected: device::Device *_interface; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 1bcf8b5ece..8928f8794b 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -669,6 +669,4 @@ MPU9250::print_status() perf_print_counter(_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); - - _mag.print_status(); } diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index f824a1350e..8f7c774d55 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -84,8 +84,6 @@ void AK09916::print_status() perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_magnetic_sensor_overflow_perf); - - _px4_mag.print_status(); } int AK09916::probe() diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp index edef0aa071..643b6ee394 100644 --- a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp @@ -84,8 +84,6 @@ void AK8963::print_status() perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_magnetic_sensor_overflow_perf); - - _px4_mag.print_status(); } int AK8963::probe() diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp index 3c478b5d07..87dfd42159 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ b/src/drivers/magnetometer/bmm150/bmm150.cpp @@ -549,7 +549,6 @@ void BMM150::print_status() perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); perf_print_counter(_good_transfers); - _px4_mag.print_status(); } int BMM150::self_test() diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp index 6754d0f480..539a9d331f 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp @@ -482,5 +482,4 @@ void HMC5883::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("interval: %u us\n", _measure_interval); - _px4_mag.print_status(); } diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.hpp b/src/drivers/magnetometer/hmc5883/HMC5883.hpp index 0d77457c49..dff0600a39 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.hpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.hpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp index d873578e7c..8fa6e23cff 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include "hmc5883.h" diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp index 514269e6de..d1e1303349 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include "hmc5883.h" diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index fad021c3b2..a036401315 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -81,7 +81,6 @@ void IST8308::print_status() perf_print_counter(_transfer_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); - _px4_mag.print_status(); } int IST8308::probe() diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index 698c4d4254..4f17ca8520 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include @@ -581,7 +580,6 @@ void IST8310::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("poll interval: %u interval\n", _measure_interval); - _px4_mag.print_status(); } I2CSPIDriverBase * diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp index e8ee7b551d..d757b2458a 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp @@ -163,7 +163,6 @@ LIS2MDL::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); PX4_INFO("poll interval: %u", _measure_interval); - _px4_mag.print_status(); } void diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.h b/src/drivers/magnetometer/lis2mdl/lis2mdl.h index cd5bf359b0..43e6705545 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.h +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.h @@ -41,7 +41,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp index a8691909e1..7f721da2ec 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp index 2d0a1776f5..151a7b0a53 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index 242cd631fb..4ba6354862 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -195,7 +195,6 @@ void LIS3MDL::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); PX4_INFO("poll interval: %u", _measure_interval); - _px4_mag.print_status(); } int LIS3MDL::reset() diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index 8725761da8..6d698620f8 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -41,7 +41,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index 65d1a3ac13..b39dd5664c 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index 056f154378..5d6f15cced 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 6beafd6425..2472222c7e 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -334,5 +334,4 @@ void LSM303AGR::print_status() perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); - _px4_mag.print_status(); } diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index 857abc827a..79b228c8ab 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -35,7 +35,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index 435ff79459..98ef5385bf 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -200,5 +200,4 @@ void LSM9DS1_MAG::print_status() perf_print_counter(_interval_perf); perf_print_counter(_transfer_perf); perf_print_counter(_data_overrun_perf); - _px4_mag.print_status(); } diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.cpp b/src/drivers/magnetometer/qmc5883/QMC5883.cpp index b3e72a18f4..e7b43df7dd 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883.cpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883.cpp @@ -308,5 +308,4 @@ void QMC5883::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("interval: %u us\n", _measure_interval); - _px4_mag.print_status(); } diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.hpp b/src/drivers/magnetometer/qmc5883/QMC5883.hpp index 44ed767cac..24ad93fdac 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883.hpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883.hpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp b/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp index 9586bad02b..c8a5562d52 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include "qmc5883.h" diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index 37bea380a2..0d98fdeb34 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -267,7 +267,6 @@ void RM3100::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); PX4_INFO("poll interval: %u", _measure_interval); - _px4_mag.print_status(); } int RM3100::reset() diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index 4caae1e0f3..ee10595ee4 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -41,7 +41,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index 7133f8df41..6c69505b91 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp index f39d524d31..6aa7dd031a 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include #include "board_config.h" diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 58eb69126d..a17798db07 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -51,7 +51,6 @@ add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) add_subdirectory(led) -add_subdirectory(mag_compensation) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index 742b1e4265..cb3350829a 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -38,13 +38,14 @@ PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : CDev(nullptr), - _sensor_mag_pub{ORB_ID(sensor_mag), priority}, - _rotation{rotation}, - _device_id{device_id} + _sensor_pub{ORB_ID(sensor_mag), priority}, + _device_id{device_id}, + _rotation{rotation} { - _class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + // advertise immediately to keep instance numbering in sync + _sensor_pub.advertise(); - _sensor_mag_pub.advertise(); + _class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH); } PX4Magnetometer::~PX4Magnetometer() @@ -53,46 +54,7 @@ PX4Magnetometer::~PX4Magnetometer() unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance); } - _sensor_mag_pub.unadvertise(); -} - -int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case MAGIOCSSCALE: { - // Copy offsets and scale factors in - mag_calibration_s cal{}; - memcpy(&cal, (mag_calibration_s *) arg, sizeof(cal)); - - _calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; - _calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; - } - - return PX4_OK; - - case MAGIOCGSCALE: { - // copy out scale factors - mag_calibration_s cal{}; - cal.x_offset = _calibration_offset(0); - cal.y_offset = _calibration_offset(1); - cal.z_offset = _calibration_offset(2); - cal.x_scale = _calibration_scale(0); - cal.y_scale = _calibration_scale(1); - cal.z_scale = _calibration_scale(2); - memcpy((mag_calibration_s *)arg, &cal, sizeof(cal)); - } - - return 0; - - case MAGIOCGEXTERNAL: - return _external; - - case DEVIOCGDEVICEID: - return _device_id; - - default: - return -ENOTTY; - } + _sensor_pub.unadvertise(); } void PX4Magnetometer::set_device_type(uint8_t devtype) @@ -104,11 +66,11 @@ void PX4Magnetometer::set_device_type(uint8_t devtype) // update to new device type device_id.devid_s.devtype = devtype; - // copy back to report + // copy back _device_id = device_id.devid; } -void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z) +void PX4Magnetometer::update(const hrt_abstime ×tamp_sample, float x, float y, float z) { sensor_mag_s report; report.timestamp_sample = timestamp_sample; @@ -119,27 +81,12 @@ void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, flo // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); - const matrix::Vector3f raw_f{x, y, z}; - - // Apply range scale and the calibrating offset/scale - const matrix::Vector3f val_calibrated{(((raw_f * _scale) - _calibration_offset).emult(_calibration_scale))}; - - report.x = val_calibrated(0); - report.y = val_calibrated(1); - report.z = val_calibrated(2); + report.x = x * _scale; + report.y = y * _scale; + report.z = z * _scale; report.is_external = _external; report.timestamp = hrt_absolute_time(); - _sensor_mag_pub.publish(report); -} - -void PX4Magnetometer::print_status() -{ - PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); - - PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1), - (double)_calibration_scale(2)); - PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1), - (double)_calibration_offset(2)); + _sensor_pub.publish(report); } diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 0a7519c695..c8f4daa6fa 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -47,10 +46,9 @@ public: PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Magnetometer() override; - int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; - bool external() { return _external; } + void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); void set_error_count(uint32_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } @@ -58,24 +56,19 @@ public: void set_temperature(float temperature) { _temperature = temperature; } void set_external(bool external) { _external = external; } - void update(hrt_abstime timestamp_sample, float x, float y, float z); + void update(const hrt_abstime ×tamp_sample, float x, float y, float z); int get_class_instance() { return _class_device_instance; }; - void print_status(); - private: - uORB::PublicationMulti _sensor_mag_pub; + uORB::PublicationMulti _sensor_pub; - matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f}; - matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; + uint32_t _device_id{0}; + const enum Rotation _rotation; - const enum Rotation _rotation; - uint32_t _device_id{0}; - float _temperature{NAN}; - float _scale{1.f}; - - uint32_t _error_count{0}; + float _scale{1.f}; + float _temperature{NAN}; + uint32_t _error_count{0}; bool _external{false}; diff --git a/src/lib/mag_compensation/MagCompensation.cpp b/src/lib/mag_compensation/MagCompensation.cpp deleted file mode 100644 index 6903b9384a..0000000000 --- a/src/lib/mag_compensation/MagCompensation.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 MagCompensation.cpp - * - */ - -#include "MagCompensation.hpp" -#include - -MagCompensator::MagCompensator(ModuleParams *parent): - ModuleParams(parent) -{ - -} - -void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f ¶m_vect) -{ - if (_armed) { - mag = mag + param_vect * _power; - } -} diff --git a/src/lib/mag_compensation/MagCompensation.hpp b/src/lib/mag_compensation/MagCompensation.hpp deleted file mode 100644 index 552d0a482f..0000000000 --- a/src/lib/mag_compensation/MagCompensation.hpp +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 MagCompensation.hpp - * @author Roman Bapst - * - * Library for magnetometer data compensation. - * - */ - -#pragma once - -#include -#include - -class MagCompensator : public ModuleParams -{ -public: - MagCompensator(ModuleParams *parent); - - ~MagCompensator() = default; - - void update_armed_flag(bool armed) { _armed = armed; } - - void update_power(float power) { _power = power; } - - void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f ¶m_vect); - -private: - float _power{0}; - bool _armed{false}; -}; diff --git a/src/lib/mag_compensation/mag_compensation_params.c b/src/lib/mag_compensation/mag_compensation_params.c deleted file mode 100644 index 8ec9b72983..0000000000 --- a/src/lib/mag_compensation/mag_compensation_params.c +++ /dev/null @@ -1,44 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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. - * - ****************************************************************************/ -/** - * Type of magnetometer compensation - * - * @value 0 Disabled - * @value 1 Throttle-based compensation - * @value 2 Current-based compensation (battery_status instance 0) - * @value 3 Current-based compensation (battery_status instance 1) - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 1c4884bc45..8942b7bcda 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -87,11 +87,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (sys_has_mag == 1) { - bool prime_found = false; - - int32_t prime_id = -1; - param_get(param_find("CAL_MAG_PRIME"), &prime_id); - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); @@ -99,29 +94,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t device_id = -1; - if (magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { + if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { if (required) { failed = true; } } } - - /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status); - failed = true; - } + // TODO: highest priority mag /* mag consistency checks (need to be performed after the individual checks) */ if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) { @@ -132,10 +112,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- ACCEL ---- */ if (checkSensors) { - bool prime_found = false; - int32_t prime_id = -1; - param_get(param_find("CAL_ACC_PRIME"), &prime_id); - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { const bool required = (i < max_mandatory_accel_count); @@ -143,63 +119,31 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t device_id = -1; - if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { + if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) { if (required) { failed = true; } } } - /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status); - failed = true; - } + // TODO: highest priority (from params) } /* ---- GYRO ---- */ if (checkSensors) { - bool prime_found = false; - int32_t prime_id = -1; - param_get(param_find("CAL_GYRO_PRIME"), &prime_id); - /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { const bool required = (i < max_mandatory_gyro_count); int32_t device_id = -1; - if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { + if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) { if (required) { failed = true; } } } - /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status); - failed = true; - } + // TODO: highest priority (from params) } /* ---- BARO ---- */ diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp index cd81a0755f..75e338249d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, @@ -47,9 +47,9 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_ float test_limit = 1.0f; // pass limit re-used for each test // Get sensor_preflight data if available and exit with a fail recorded if not - uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; + uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight_imu)}; sensors_sub.update(); - const sensor_preflight_s &sensors = sensors_sub.get(); + const sensor_preflight_imu_s &sensors = sensors_sub.get(); // Use the difference between IMU's to detect a bad calibration. // If a single IMU is fitted, the value being checked will be zero so this check will always pass. diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp index bc4a077dd6..8f0d16993f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include // return false if the magnetomer measurements are inconsistent bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, @@ -49,9 +49,9 @@ bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_ bool pass = false; // flag for result of checks // get the sensor preflight data - uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; + uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight_mag)}; sensors_sub.update(); - const sensor_preflight_s &sensors = sensors_sub.get(); + const sensor_preflight_mag_s &sensors = sensors_sub.get(); if (sensors.timestamp == 0) { // can happen if not advertised (yet) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4e3a0ed6c6..3bf620d78c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -886,8 +886,8 @@ void Ekf2::Run() // Do not reset parmameters when armed to prevent potential time slips casued by parameter set // and notification events // Check if there has been a persistant change in magnetometer ID - if (_sensor_selection.mag_device_id != 0 - && (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) { + if (magnetometer.device_id != 0 + && (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) { if (_invalid_mag_id_count < 200) { _invalid_mag_id_count++; @@ -908,7 +908,7 @@ void Ekf2::Run() _param_ekf2_magbias_y.commit_no_notification(); _param_ekf2_magbias_z.set(0.f); _param_ekf2_magbias_z.commit_no_notification(); - _param_ekf2_magbias_id.set(_sensor_selection.mag_device_id); + _param_ekf2_magbias_id.set(magnetometer.device_id); _param_ekf2_magbias_id.commit(); _invalid_mag_id_count = 0; @@ -1510,7 +1510,7 @@ void Ekf2::Run() bias.accel_device_id = _sensor_selection.accel_device_id; } - bias.mag_device_id = _sensor_selection.mag_device_id; + bias.mag_device_id = _param_ekf2_magbias_id.get(); // In-run bias estimates _ekf.getGyroBias().copyTo(bias.gyro_bias); @@ -1632,8 +1632,7 @@ void Ekf2::Run() // Check and save the last valid calibration when we are disarmed if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) - && (status.filter_fault_flags == 0) - && (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) { + && (status.filter_fault_flags == 0)) { update_mag_bias(_param_ekf2_magbias_x, 0); update_mag_bias(_param_ekf2_magbias_y, 1); @@ -1642,7 +1641,6 @@ void Ekf2::Run() // reset to prevent data being saved too frequently _total_cal_time_us = 0; } - } publish_wind_estimate(now); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8e9b3834d2..e007449a66 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,7 +79,8 @@ void LoggedTopics::add_default_topics() add_topic("safety"); add_topic("sensor_combined"); add_topic("sensor_correction"); - add_topic("sensor_preflight", 200); + add_topic("sensor_preflight_imu", 200); + add_topic("sensor_preflight_mag", 500); add_topic("sensor_selection"); add_topic("system_power", 500); add_topic("tecs_status", 200); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 50e020a165..5b8398b14d 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) add_subdirectory(vehicle_air_data) add_subdirectory(vehicle_imu) +add_subdirectory(vehicle_magnetometer) px4_add_module( MODULE modules__sensors @@ -45,7 +46,6 @@ px4_add_module( SRCS voted_sensors_update.cpp sensors.cpp - parameters.cpp DEPENDS airspeed conversion @@ -58,5 +58,5 @@ px4_add_module( vehicle_angular_velocity vehicle_air_data vehicle_imu - mag_compensation + vehicle_magnetometer ) diff --git a/src/modules/sensors/common.h b/src/modules/sensors/common.h deleted file mode 100644 index bb6e5a31a3..0000000000 --- a/src/modules/sensors/common.h +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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. - * - ****************************************************************************/ - -#pragma once - -/** - * @file common.h - * common definitions used in sensors module - * - * @author Beat Kueng - */ - -namespace sensors -{ - -constexpr uint8_t MAG_COUNT_MAX = 4; -constexpr uint8_t GYRO_COUNT_MAX = 3; -constexpr uint8_t ACCEL_COUNT_MAX = 3; - -constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX)); - -} /* namespace sensors */ diff --git a/src/modules/sensors/data_validator/tests/tests_common.cpp b/src/modules/sensors/data_validator/tests/tests_common.cpp index bcf18ba369..78848c1e7e 100644 --- a/src/modules/sensors/data_validator/tests/tests_common.cpp +++ b/src/modules/sensors/data_validator/tests/tests_common.cpp @@ -100,4 +100,4 @@ void fill_validator_with_samples(DataValidator *validator, *timestamp_io = timestamp; *value_io = val; -} \ No newline at end of file +} diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp deleted file mode 100644 index aa0f187977..0000000000 --- a/src/modules/sensors/parameters.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 parameters.cpp - * - * @author Beat Kueng - */ - -#include "parameters.h" - -namespace sensors -{ - -void initialize_parameter_handles(ParameterHandles ¶meter_handles) -{ - /* Differential pressure offset */ - parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - /* rotations */ - parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); - - /* rotation offsets */ - parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); - parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); - parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - - /* mag compensation */ - parameter_handles.mag_comp_type = param_find("CAL_MAG_COMP_TYP"); - - parameter_handles.mag_comp_paramX[0] = param_find("CAL_MAG0_XCOMP"); - parameter_handles.mag_comp_paramX[1] = param_find("CAL_MAG1_XCOMP"); - parameter_handles.mag_comp_paramX[2] = param_find("CAL_MAG2_XCOMP"); - parameter_handles.mag_comp_paramX[3] = param_find("CAL_MAG3_XCOMP"); - - parameter_handles.mag_comp_paramY[0] = param_find("CAL_MAG0_YCOMP"); - parameter_handles.mag_comp_paramY[1] = param_find("CAL_MAG1_YCOMP"); - parameter_handles.mag_comp_paramY[2] = param_find("CAL_MAG2_YCOMP"); - parameter_handles.mag_comp_paramY[3] = param_find("CAL_MAG3_YCOMP"); - - parameter_handles.mag_comp_paramZ[0] = param_find("CAL_MAG0_ZCOMP"); - parameter_handles.mag_comp_paramZ[1] = param_find("CAL_MAG1_ZCOMP"); - parameter_handles.mag_comp_paramZ[2] = param_find("CAL_MAG2_ZCOMP"); - parameter_handles.mag_comp_paramZ[3] = param_find("CAL_MAG3_ZCOMP"); - - parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); - parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); - parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - - (void)param_find("BAT_V_DIV"); - (void)param_find("BAT_A_PER_V"); - - (void)param_find("CAL_ACC0_ID"); - (void)param_find("CAL_GYRO0_ID"); - - (void)param_find("CAL_MAG0_ID"); - (void)param_find("CAL_MAG1_ID"); - (void)param_find("CAL_MAG2_ID"); - (void)param_find("CAL_MAG3_ID"); - (void)param_find("CAL_MAG0_ROT"); - (void)param_find("CAL_MAG1_ROT"); - (void)param_find("CAL_MAG2_ROT"); - (void)param_find("CAL_MAG3_ROT"); - (void)param_find("CAL_MAG_SIDES"); - - (void)param_find("SYS_PARAM_VER"); - (void)param_find("SYS_AUTOSTART"); - (void)param_find("SYS_AUTOCONFIG"); - (void)param_find("TRIG_MODE"); - (void)param_find("UAVCAN_ENABLE"); - - // Parameters controlling the on-board sensor thermal calibrator - (void)param_find("SYS_CAL_TDEL"); - (void)param_find("SYS_CAL_TMAX"); - (void)param_find("SYS_CAL_TMIN"); -} - -void update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) -{ - /* Airspeed offset */ - param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa)); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale)); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - param_get(parameter_handles.board_rotation, &(parameters.board_rotation)); - - param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0])); - param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1])); - param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2])); - - param_get(parameter_handles.mag_comp_type, &(parameters.mag_comp_type)); - - param_get(parameter_handles.mag_comp_paramX[0], &(parameters.mag_comp_paramX[0])); - param_get(parameter_handles.mag_comp_paramX[1], &(parameters.mag_comp_paramX[1])); - param_get(parameter_handles.mag_comp_paramX[2], &(parameters.mag_comp_paramX[2])); - param_get(parameter_handles.mag_comp_paramX[3], &(parameters.mag_comp_paramX[3])); - - param_get(parameter_handles.mag_comp_paramY[0], &(parameters.mag_comp_paramY[0])); - param_get(parameter_handles.mag_comp_paramY[1], &(parameters.mag_comp_paramY[1])); - param_get(parameter_handles.mag_comp_paramY[2], &(parameters.mag_comp_paramY[2])); - param_get(parameter_handles.mag_comp_paramY[3], &(parameters.mag_comp_paramY[3])); - - param_get(parameter_handles.mag_comp_paramZ[0], &(parameters.mag_comp_paramZ[0])); - param_get(parameter_handles.mag_comp_paramZ[1], &(parameters.mag_comp_paramZ[1])); - param_get(parameter_handles.mag_comp_paramZ[2], &(parameters.mag_comp_paramZ[2])); - param_get(parameter_handles.mag_comp_paramZ[3], &(parameters.mag_comp_paramZ[3])); - - param_get(parameter_handles.air_cmodel, ¶meters.air_cmodel); - param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); - param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); -} - -} /* namespace sensors */ diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h deleted file mode 100644 index f4a86a76a3..0000000000 --- a/src/modules/sensors/parameters.h +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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. - * - ****************************************************************************/ - -#pragma once - -/** - * @file parameters.h - * - * defines the list of parameters that are used within the sensors module - * - * @author Beat Kueng - */ - -#include - -namespace sensors -{ - -struct Parameters { - float diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - float diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - int32_t board_rotation; - - float board_offset[3]; - - //parameters for current/throttle-based mag compensation - int32_t mag_comp_type; - float mag_comp_paramX[4]; - float mag_comp_paramY[4]; - float mag_comp_paramZ[4]; - - int32_t air_cmodel; - float air_tube_length; - float air_tube_diameter_mm; -}; - -struct ParameterHandles { - param_t diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_t diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - param_t board_rotation; - - param_t board_offset[3]; - - param_t mag_comp_type; - param_t mag_comp_paramX[4]; - param_t mag_comp_paramY[4]; - param_t mag_comp_paramZ[4]; - - param_t air_cmodel; - param_t air_tube_length; - param_t air_tube_diameter_mm; - -}; - -/** - * initialize ParameterHandles struct - */ -void initialize_parameter_handles(ParameterHandles ¶meter_handles); - - -/** - * Read out the parameters using the handles into the parameters struct. - * @return 0 on success, <0 on error - */ -void update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); - -} /* namespace sensors */ diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index f79cd6a9e9..6c9d29f30f 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -62,3 +62,32 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); * @group Sensors */ PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63); + +/** + * Type of magnetometer compensation + * + * @value 0 Disabled + * @value 1 Throttle-based compensation + * @value 2 Current-based compensation (battery_status instance 0) + * @value 3 Current-based compensation (battery_status instance 1) + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0); + +/** + * Magnetometer max rate. + * + * Magnetometer data maximum publication rate. This is an upper bound, + * actual magnetometer data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f); diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index a5829c3d3e..4638c9e126 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/src/modules/sensors/sensor_params_mag0.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 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 @@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f); +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0f); + /** * Coefficient describing linear relationship between * X component of magnetometer in body frame axis diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index e76e575bad..3418e2c2ee 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/src/modules/sensors/sensor_params_mag1.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 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 @@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f); +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0f); + /** * Coefficient describing linear relationship between * X component of magnetometer in body frame axis diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index a2c55ba99b..2f575860ed 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/src/modules/sensors/sensor_params_mag2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 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 @@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f); +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0f); + /** * Coefficient describing linear relationship between * X component of magnetometer in body frame axis diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index df2e0160c2..c6d979ab21 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/src/modules/sensors/sensor_params_mag3.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 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 @@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f); +/** + * Magnetometer X-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0f); + +/** + * Magnetometer Y-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0f); + +/** + * Magnetometer Z-axis off diagonal factor + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0f); + /** * Coefficient describing linear relationship between * X component of magnetometer in body frame axis diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42f50a7d56..1df10d7555 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -65,19 +64,17 @@ #include #include #include -#include +#include #include #include #include -#include -#include -#include "parameters.h" #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" #include "vehicle_air_data/VehicleAirData.hpp" #include "vehicle_imu/VehicleIMU.hpp" +#include "vehicle_magnetometer/VehicleMagnetometer.hpp" using namespace sensors; using namespace time_literals; @@ -116,10 +113,9 @@ private: hrt_abstime _last_config_update{0}; hrt_abstime _sensor_combined_prev_timestamp{0}; - hrt_abstime _magnetometer_prev_timestamp{0}; sensor_combined_s _sensor_combined{}; - sensor_preflight_s _sensor_preflight{}; + sensor_preflight_imu_s _sensor_preflight_imu{}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] { {this, ORB_ID(vehicle_imu), 0}, @@ -127,26 +123,14 @@ private: {this, ORB_ID(vehicle_imu), 2} }; - uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ - uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */ - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; /**< battery_status instance 0 subscription */ + uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ - uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */ - uORB::Publication _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */ - uORB::Publication _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */ - - enum class MagCompensationType { - Disabled = 0, - Throttle, - Current_inst0, - Current_inst1 - }; - - MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; + uORB::Publication _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)}; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -161,14 +145,35 @@ private: uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */ #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - Parameters _parameters{}; /**< local copies of interesting parameters */ - ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ + + struct Parameters { + float diff_pres_offset_pa; +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + float diff_pres_analog_scale; +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + int32_t air_cmodel; + float air_tube_length; + float air_tube_diameter_mm; + } _parameters{}; /**< local copies of interesting parameters */ + + struct ParameterHandles { + param_t diff_pres_offset_pa; +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + param_t diff_pres_analog_scale; +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + param_t air_cmodel; + param_t air_tube_length; + param_t air_tube_diameter_mm; + } _parameter_handles{}; /**< handles for interesting parameters */ VotedSensorsUpdate _voted_sensors_update; VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; VehicleAirData *_vehicle_air_data{nullptr}; + VehicleMagnetometer *_vehicle_magnetometer{nullptr}; static constexpr int MAX_SENSOR_COUNT = 3; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -202,9 +207,11 @@ private: void InitializeVehicleAirData(); void InitializeVehicleIMU(); + void InitializeVehicleMagnetometer(); DEFINE_PARAMETERS( - (ParamBool) _param_sys_has_baro + (ParamBool) _param_sys_has_baro, + (ParamBool) _param_sys_has_mag ) }; @@ -213,9 +220,39 @@ Sensors::Sensors(bool hil_enabled) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _hil_enabled(hil_enabled), _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub) + _voted_sensors_update(hil_enabled, _vehicle_imu_sub) { - initialize_parameter_handles(_parameter_handles); + /* Differential pressure offset */ + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); + _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); + _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); + + param_find("BAT_V_DIV"); + param_find("BAT_A_PER_V"); + + param_find("CAL_ACC0_ID"); + param_find("CAL_GYRO0_ID"); + + param_find("SENS_BOARD_ROT"); + param_find("SENS_BOARD_X_OFF"); + param_find("SENS_BOARD_Y_OFF"); + param_find("SENS_BOARD_Z_OFF"); + + param_find("SYS_PARAM_VER"); + param_find("SYS_AUTOSTART"); + param_find("SYS_AUTOCONFIG"); + param_find("TRIG_MODE"); + param_find("UAVCAN_ENABLE"); + + // Parameters controlling the on-board sensor thermal calibrator + param_find("SYS_CAL_TDEL"); + param_find("SYS_CAL_TMAX"); + param_find("SYS_CAL_TMIN"); _airspeed_validator.set_timeout(300000); _airspeed_validator.set_equal_value_threshold(100); @@ -239,6 +276,11 @@ Sensors::~Sensors() delete _vehicle_air_data; } + if (_vehicle_magnetometer) { + _vehicle_magnetometer->Stop(); + delete _vehicle_magnetometer; + } + for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { vehicle_imu->Stop(); @@ -265,8 +307,15 @@ int Sensors::parameters_update() return 0; } - /* read the parameter values into _parameters */ - update_parameters(_parameter_handles, _parameters); + /* Airspeed offset */ + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); +#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ + + param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); + param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); + param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); _voted_sensors_update.parametersUpdate(); @@ -474,6 +523,21 @@ void Sensors::InitializeVehicleIMU() } } +void Sensors::InitializeVehicleMagnetometer() +{ + if (_param_sys_has_mag.get()) { + if (_vehicle_magnetometer == nullptr) { + if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) { + _vehicle_magnetometer = new VehicleMagnetometer(); + + if (_vehicle_magnetometer) { + _vehicle_magnetometer->Start(); + } + } + } + } +} + void Sensors::Run() { if (should_exit()) { @@ -490,6 +554,7 @@ void Sensors::Run() if (_last_config_update == 0) { InitializeVehicleAirData(); InitializeVehicleIMU(); + InitializeVehicleMagnetometer(); _voted_sensors_update.init(_sensor_combined); parameter_update_poll(true); } @@ -505,57 +570,16 @@ void Sensors::Run() if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { _armed = vcontrol_mode.flag_armed; - _voted_sensors_update.update_mag_comp_armed(_armed); - } - - //check mag power compensation type (change battery current subscription instance if necessary) - if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0 - && _mag_comp_type != MagCompensationType::Current_inst0) { - _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0}; - } - - if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1 - && _mag_comp_type != MagCompensationType::Current_inst1) { - _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1}; - } - - _mag_comp_type = (MagCompensationType)_parameters.mag_comp_type; - - //update power signal for mag compensation - if (_mag_comp_type == MagCompensationType::Throttle) { - actuator_controls_s controls {}; - - if (_actuator_ctrl_0_sub.update(&controls)) { - _voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]); - } - - } else if (_mag_comp_type == MagCompensationType::Current_inst0 - || _mag_comp_type == MagCompensationType::Current_inst1) { - battery_status_s bat_stat {}; - - if (_battery_status_sub.update(&bat_stat)) { - _voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA] - } } } - vehicle_magnetometer_s magnetometer{}; - _voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer); + _voted_sensors_update.sensorsPoll(_sensor_combined); // check analog airspeed adc_poll(); diff_pres_poll(); - if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) { - _magnetometer_pub.publish(magnetometer); - _magnetometer_prev_timestamp = magnetometer.timestamp; - - if (!_armed) { - _voted_sensors_update.calcMagInconsistency(_sensor_preflight); - } - } - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { _voted_sensors_update.setRelativeTimestamps(_sensor_combined); @@ -565,11 +589,11 @@ void Sensors::Run() // If the the vehicle is disarmed calculate the length of the maximum difference between // IMU units as a consistency metric and publish to the sensor preflight topic if (!_armed) { - _voted_sensors_update.calcAccelInconsistency(_sensor_preflight); - _voted_sensors_update.calcGyroInconsistency(_sensor_preflight); + _voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu); + _voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu); - _sensor_preflight.timestamp = hrt_absolute_time(); - _sensor_preflight_pub.publish(_sensor_preflight); + _sensor_preflight_imu.timestamp = hrt_absolute_time(); + _sensor_preflight_imu_pub.publish(_sensor_preflight_imu); } } @@ -579,6 +603,7 @@ void Sensors::Run() _voted_sensors_update.initializeSensors(); InitializeVehicleAirData(); InitializeVehicleIMU(); + InitializeVehicleMagnetometer(); _last_config_update = hrt_absolute_time(); } else { @@ -644,6 +669,11 @@ int Sensors::print_status() { _voted_sensors_update.printStatus(); + if (_vehicle_magnetometer) { + PX4_INFO_RAW("\n"); + _vehicle_magnetometer->PrintStatus(); + } + if (_vehicle_air_data) { PX4_INFO_RAW("\n"); _vehicle_air_data->PrintStatus(); @@ -696,7 +726,7 @@ The provided functionality includes: - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the sensor drivers must already be running when `sensors` is started. -- Do preflight sensor consistency checks and publish the `sensor_preflight` topic. +- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic. ### Implementation It runs in its own thread and polls on the currently selected gyro topic. diff --git a/src/lib/mag_compensation/CMakeLists.txt b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt similarity index 86% rename from src/lib/mag_compensation/CMakeLists.txt rename to src/modules/sensors/vehicle_magnetometer/CMakeLists.txt index aee68a89c9..a864b955e2 100644 --- a/src/lib/mag_compensation/CMakeLists.txt +++ b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 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 @@ -31,4 +31,13 @@ # ############################################################################ -px4_add_library(mag_compensation MagCompensation.cpp) +px4_add_library(vehicle_magnetometer + VehicleMagnetometer.cpp + VehicleMagnetometer.hpp +) + +target_link_libraries(vehicle_magnetometer + PRIVATE + px4_work_queue + sensor_calibration +) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp new file mode 100644 index 0000000000..7d864be9ba --- /dev/null +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -0,0 +1,383 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "VehicleMagnetometer.hpp" + +#include +#include + +namespace sensors +{ + +using namespace matrix; +using namespace time_literals; + +static constexpr int32_t MAG_ROT_VAL_INTERNAL{-1}; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleMagnetometer::VehicleMagnetometer() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + char str[20] {}; + + for (int mag_index = 0; mag_index < 4; mag_index++) { + // CAL_MAGx_ID + sprintf(str, "CAL_%s%u_ID", "MAG", mag_index); + param_find(str); + + // CAL_MAGx_ROT + sprintf(str, "CAL_%s%u_ROT", "MAG", mag_index); + param_find(str); + } + + param_find("CAL_MAG_SIDES"); + param_find("CAL_MAG_ROT_AUTO"); + + _voter.set_timeout(SENSOR_TIMEOUT); + _voter.set_equal_value_threshold(1000); + + ParametersUpdate(true); +} + +VehicleMagnetometer::~VehicleMagnetometer() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleMagnetometer::Start() +{ + ScheduleNow(); + return true; +} + +void VehicleMagnetometer::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } +} + +void VehicleMagnetometer::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + // Mag compensation type + MagCompensationType mag_comp_typ = static_cast(_param_mag_comp_typ.get()); + + if (mag_comp_typ != _mag_comp_type) { + // check mag power compensation type (change battery current subscription instance if necessary) + if (mag_comp_typ == MagCompensationType::Current_inst0 && _mag_comp_type != MagCompensationType::Current_inst0) { + _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0}; + } + + if (mag_comp_typ == MagCompensationType::Current_inst1 && _mag_comp_type != MagCompensationType::Current_inst1) { + _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1}; + } + + if (mag_comp_typ == MagCompensationType::Throttle) { + _actuator_controls_0_sub = uORB::Subscription{ORB_ID(actuator_controls_0)}; + } + } + + _mag_comp_type = mag_comp_typ; + } +} + +void VehicleMagnetometer::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + // check vehicle status for changes to armed state + if (_vcontrol_mode_sub.updated()) { + vehicle_control_mode_s vcontrol_mode; + + if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { + _armed = vcontrol_mode.flag_armed; + } + } + + if (_mag_comp_type != MagCompensationType::Disabled) { + // update power signal for mag compensation + if (_armed) { + if (_mag_comp_type == MagCompensationType::Throttle) { + actuator_controls_s controls; + + if (_actuator_controls_0_sub.update(&controls)) { + for (auto &cal : _calibration) { + cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]); + } + } + + } else if (_mag_comp_type == MagCompensationType::Current_inst0 + || _mag_comp_type == MagCompensationType::Current_inst1) { + + battery_status_s bat_stat; + + if (_battery_status_sub.update(&bat_stat)) { + float power = bat_stat.current_a * 0.001f; //current in [kA] + + for (auto &cal : _calibration) { + cal.UpdatePower(power); + } + } + } + + } else { + for (auto &cal : _calibration) { + cal.UpdatePower(0.f); + } + } + } + + bool updated[MAX_SENSOR_COUNT] {}; + + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + + if (!_calibration[uorb_index].enabled()) { + continue; + } + + if (!_advertised[uorb_index]) { + // use data's timestamp to throttle advertisement checks + if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) { + if (_sensor_sub[uorb_index].advertised()) { + if (uorb_index > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (!_voter.add_new_validator()) { + PX4_ERR("failed to add validator for %s %i", "MAG", uorb_index); + } + } + + _advertised[uorb_index] = true; + + } else { + _last_data[uorb_index].timestamp = hrt_absolute_time(); + } + } + + } else { + sensor_mag_s report; + updated[uorb_index] = _sensor_sub[uorb_index].update(&report); + + if (updated[uorb_index]) { + if (_calibration[uorb_index].device_id() != report.device_id) { + _calibration[uorb_index].set_external(report.is_external); + _calibration[uorb_index].set_device_id(report.device_id); + _priority[uorb_index] = _sensor_sub[uorb_index].get_priority(); + } + + if (_calibration[uorb_index].enabled()) { + Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}); + + _last_data[uorb_index].timestamp_sample = report.timestamp_sample; + _last_data[uorb_index].device_id = report.device_id; + _last_data[uorb_index].x = vect(0); + _last_data[uorb_index].y = vect(1); + _last_data[uorb_index].z = vect(2); + + float mag_array[3] {vect(0), vect(1), vect(2)}; + _voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]); + } + } + } + } + + // check for the current best sensor + int best_index = 0; + _voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + if (_selected_sensor_sub_index != best_index) { + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } + + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index); + } + + _selected_sensor_sub_index = best_index; + _sensor_sub[_selected_sensor_sub_index].registerCallback(); + } + } + + if ((_selected_sensor_sub_index >= 0) + && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR) + && updated[_selected_sensor_sub_index]) { + + const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index]; + + _mag_timestamp_sum += mag.timestamp_sample; + _mag_sum += Vector3f{mag.x, mag.y, mag.z}; + _mag_sum_count++; + + if ((_param_sens_mag_rate.get() > 0) + && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) { + + const Vector3f magnetometer_data = _mag_sum / _mag_sum_count; + const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count; + + // reset + _mag_timestamp_sum = 0; + _mag_sum.zero(); + _mag_sum_count = 0; + + // populate vehicle_magnetometer with primary mag and publish + vehicle_magnetometer_s out{}; + out.timestamp_sample = timestamp_sample; + out.device_id = mag.device_id; + magnetometer_data.copyTo(out.magnetometer_ga); + + out.timestamp = hrt_absolute_time(); + _vehicle_magnetometer_pub.publish(out); + + _last_publication_timestamp = out.timestamp; + } + } + + // check failover and report + if (_last_failover_count != _voter.failover_count()) { + uint32_t flags = _voter.failover_state(); + int failover_index = _voter.failover_index(); + + if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { + if (failover_index != -1) { + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!", + "MAG", + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } + + // reduce priority of failed sensor to the minimum + _priority[failover_index] = 1; + } + } + + _last_failover_count = _voter.failover_count(); + } + + if (!_armed) { + calcMagInconsistency(); + } + + // reschedule timeout + ScheduleDelayed(100_ms); + + perf_end(_cycle_perf); +} + +void VehicleMagnetometer::calcMagInconsistency() +{ + sensor_preflight_mag_s preflt{}; + + const sensor_mag_s &primary_mag_report = _last_data[_selected_sensor_sub_index]; + const Vector3f primary_mag(primary_mag_report.x, primary_mag_report.y, + primary_mag_report.z); // primary mag field vector + + float mag_angle_diff_max = 0.0f; // the maximum angle difference + unsigned check_index = 0; // the number of sensors the primary has been checked against + + // Check each sensor against the primary + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + // check that the sensor we are checking against is not the same as the primary + if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) { + // calculate angle to 3D magnetic field vector of the primary sensor + const sensor_mag_s ¤t_mag_report = _last_data[i]; + Vector3f current_mag{current_mag_report.x, current_mag_report.y, current_mag_report.z}; + + float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle(); + + // complementary filter to not fail/pass on single outliers + _mag_angle_diff[check_index] *= 0.95f; + _mag_angle_diff[check_index] += 0.05f * angle_error; + + mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); + + // increment the check index + check_index++; + } + + // check to see if the maximum number of checks has been reached and break + if (check_index >= 2) { + break; + } + } + + // get the vector length of the largest difference and write to the combined sensor struct + // will be zero if there is only one magnetometer and hence nothing to compare + preflt.mag_inconsistency_angle = mag_angle_diff_max; + + preflt.timestamp = hrt_absolute_time(); + _sensor_preflight_mag_pub.publish(preflt); +} + +void VehicleMagnetometer::PrintStatus() +{ + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("selected magnetometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, + _selected_sensor_sub_index); + } + + _voter.print(); + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_advertised[i] && (_priority[i] > 0)) { + _calibration[i].PrintStatus(); + } + } +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp new file mode 100644 index 0000000000..acc36d51da --- /dev/null +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#pragma once + +#include "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sensors +{ +class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleMagnetometer(); + ~VehicleMagnetometer() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + + /** + * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers + */ + void calcMagInconsistency(); + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::Publication _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)}; + uORB::Publication _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)}; + + uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(sensor_mag), 0}, + {this, ORB_ID(sensor_mag), 1}, + {this, ORB_ID(sensor_mag), 2}, + {this, ORB_ID(sensor_mag), 3} + }; + + calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; + + // Magnetometer interference compensation + enum class MagCompensationType { + Disabled = 0, + Throttle, + Current_inst0, + Current_inst1 + }; + MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + hrt_abstime _last_publication_timestamp{0}; + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + DataValidatorGroup _voter{1}; + unsigned _last_failover_count{0}; + + uint64_t _mag_timestamp_sum{0}; + matrix::Vector3f _mag_sum{}; + int _mag_sum_count{0}; + + sensor_mag_s _last_data[MAX_SENSOR_COUNT] {}; + bool _advertised[MAX_SENSOR_COUNT] {}; + + float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ + + uint8_t _priority[MAX_SENSOR_COUNT] {}; + + int8_t _selected_sensor_sub_index{-1}; + + bool _armed{false}; /**< arming status of the vehicle */ + + DEFINE_PARAMETERS( + (ParamInt) _param_mag_comp_typ, + (ParamFloat) _param_sens_mag_rate + ) +}; +}; // namespace sensors diff --git a/src/lib/mag_compensation/python/mag_compensation.py b/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py similarity index 100% rename from src/lib/mag_compensation/python/mag_compensation.py rename to src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 1049b3737c..72de36fdef 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -39,32 +39,20 @@ #include "voted_sensors_update.h" -#include +#include #include #include #include -#include -#include - -#define MAG_ROT_VAL_INTERNAL -1 -#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" using namespace sensors; using namespace matrix; using namespace time_literals; -using math::radians; -VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled, - uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) : +VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) : ModuleParams(nullptr), _vehicle_imu_sub(vehicle_imu_sub), - _parameters(parameters), - _hil_enabled(hil_enabled), - _mag_compensator(this) + _hil_enabled(hil_enabled) { - _mag.voter.set_timeout(300000); - _mag.voter.set_equal_value_threshold(1000); - if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit _gyro.voter.set_timeout(500000); _accel.voter.set_timeout(500000); @@ -87,26 +75,12 @@ void VotedSensorsUpdate::initializeSensors() { initSensorClass(_gyro, GYRO_COUNT_MAX); initSensorClass(_accel, ACCEL_COUNT_MAX); - initSensorClass(_mag, MAG_COUNT_MAX); } void VotedSensorsUpdate::parametersUpdate() { updateParams(); - /* fine tune board offset */ - const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}}; - - _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation); - - // initialze all mag rotations with the board rotation in case there is no calibration data available - for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) { - _mag_rotation[topic_instance] = _board_rotation; - } - - /* set offset parameters to new values */ - bool failed = false; - // run through all IMUs for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) { uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; @@ -141,159 +115,6 @@ void VotedSensorsUpdate::parametersUpdate() } } } - - - /* run through all mag sensors - * Because we store the device id in _mag_device_id, we need to get the id via uorb topic since - * the DevHandle method does not work on POSIX. - */ - - /* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers, - * and not all uORB instances have to be published yet at the initial call of parametersUpdate() - */ - for (int i = 0; i < MAG_COUNT_MAX; i++) { - _mag.enabled[i] = true; - } - - for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX - && topic_instance < _mag.subscription_count; ++topic_instance) { - - sensor_mag_s report{}; - - if (!_mag.subscription[topic_instance].copy(&report)) { - continue; - } - - uint32_t topic_device_id = report.device_id; - bool is_external = report.is_external; - _mag_device_id[topic_instance] = topic_device_id; - - // find the driver handle that matches the topic_device_id - int fd = -1; - char str[30] {}; - - for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) { - - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index); - - fd = px4_open(str, O_RDWR); - - if (fd < 0) { - /* the driver is not running, continue with the next */ - continue; - } - - uint32_t driver_device_id = (uint32_t)px4_ioctl(fd, DEVIOCGDEVICEID, 0); - - if (driver_device_id == topic_device_id) { - break; // we found the matching driver - - } else { - px4_close(fd); - } - } - - bool config_ok = false; - - /* run through all stored calibrations */ - for (unsigned i = 0; i < MAG_COUNT_MAX; i++) { - /* initially status is ok per config */ - failed = false; - - (void)sprintf(str, "CAL_MAG%u_ID", i); - int32_t device_id = 0; - failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); - - (void)sprintf(str, "CAL_MAG%u_EN", i); - int32_t device_enabled = 1; - failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled)); - - if (failed) { - continue; - } - - /* if the calibration is for this device, apply it */ - if ((uint32_t)device_id == _mag_device_id[topic_instance]) { - _mag.enabled[topic_instance] = (device_enabled == 1); - _mag.power_compensation[topic_instance] = {_parameters.mag_comp_paramX[i], _parameters.mag_comp_paramY[i], _parameters.mag_comp_paramZ[i]}; - - // the mags that were published after the initial parameterUpdate - // would be given the priority even if disabled. Reset it to 0 in this case - if (!_mag.enabled[topic_instance]) { - _mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED; - } - - mag_calibration_s mscale{}; - - (void)sprintf(str, "CAL_MAG%u_XOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_offset)); - - (void)sprintf(str, "CAL_MAG%u_YOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_offset)); - - (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_offset)); - - (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_scale)); - - (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_scale)); - - (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_scale)); - - (void)sprintf(str, "CAL_MAG%u_ROT", i); - int32_t mag_rot = 0; - param_get(param_find(str), &mag_rot); - - if (is_external) { - - /* check if this mag is still set as internal, otherwise leave untouched */ - if (mag_rot < 0) { - /* it was marked as internal, change to external with no rotation */ - mag_rot = 0; - param_set_no_notification(param_find(str), &mag_rot); - } - - } else { - /* mag is internal - reset param to -1 to indicate internal mag */ - if (mag_rot != MAG_ROT_VAL_INTERNAL) { - mag_rot = MAG_ROT_VAL_INTERNAL; - param_set_no_notification(param_find(str), &mag_rot); - } - } - - /* now get the mag rotation */ - if (mag_rot >= 0) { - // Set external magnetometers to use the parameter value - _mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot); - - } else { - // Set internal magnetometers to use the board rotation - _mag_rotation[topic_instance] = _board_rotation; - } - - if (failed) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i); - - } else { - - /* apply new scaling and offsets */ - config_ok = (px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale) == 0); - - if (!config_ok) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); - } - } - - break; - } - } - - px4_close(fd); - } - } void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) @@ -398,51 +219,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) } } -void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) -{ - for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) { - sensor_mag_s mag_report; - - if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) { - - // First publication with data - if (_mag.priority[uorb_index] == 0) { - _mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority(); - } - - Vector3f vect(mag_report.x, mag_report.y, mag_report.z); - - //throttle-/current-based mag compensation - _mag_compensator.calculate_mag_corrected(vect, _mag.power_compensation[uorb_index]); - - vect = _mag_rotation[uorb_index] * vect; - - _last_magnetometer[uorb_index].timestamp = mag_report.timestamp; - _last_magnetometer[uorb_index].magnetometer_ga[0] = vect(0); - _last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1); - _last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2); - - _mag.voter.put(uorb_index, mag_report.timestamp, _last_magnetometer[uorb_index].magnetometer_ga, mag_report.error_count, - _mag.priority[uorb_index]); - } - } - - int best_index; - _mag.voter.get_best(hrt_absolute_time(), &best_index); - - checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); - - if (best_index >= 0) { - magnetometer = _last_magnetometer[best_index]; - _mag.last_best_vote = (uint8_t)best_index; - - if (_selection.mag_device_id != _mag_device_id[best_index]) { - _selection_changed = true; - _selection.mag_device_id = _mag_device_id[best_index]; - } - } -} - bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type) { if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { @@ -494,8 +270,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; } if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; } - - if (type == subsystem_info_s::SUBSYSTEM_TYPE_MAG) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; } } _info.timestamp = hrt_absolute_time(); @@ -558,16 +332,11 @@ void VotedSensorsUpdate::printStatus() PX4_INFO_RAW("\n"); PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote); _accel.voter.print(); - - PX4_INFO_RAW("\n"); - PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote); - _mag.voter.print(); } -void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer) +void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) { imuPoll(raw); - magPoll(magnetometer); // publish sensor selection if changed if (_selection_changed) { @@ -588,8 +357,7 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) } } -void -VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt) +void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt) { float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared unsigned check_index = 0; // the number of sensors the primary has been checked against @@ -638,7 +406,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt) } } -void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt) +void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt) { float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared unsigned check_index = 0; // the number of sensors the primary has been checked against @@ -686,48 +454,3 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt) preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq); } } - -void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt) -{ - Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector - float mag_angle_diff_max = 0.0f; // the maximum angle difference - unsigned check_index = 0; // the number of sensors the primary has been checked against - - // Check each sensor against the primary - for (int i = 0; i < _mag.subscription_count; i++) { - // check that the sensor we are checking against is not the same as the primary - if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) { - // calculate angle to 3D magnetic field vector of the primary sensor - Vector3f current_mag(_last_magnetometer[i].magnetometer_ga); - float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle(); - - // complementary filter to not fail/pass on single outliers - _mag_angle_diff[check_index] *= 0.95f; - _mag_angle_diff[check_index] += 0.05f * angle_error; - - mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); - - // increment the check index - check_index++; - } - - // check to see if the maximum number of checks has been reached and break - if (check_index >= 2) { - break; - } - } - - // get the vector length of the largest difference and write to the combined sensor struct - // will be zero if there is only one magnetometer and hence nothing to compare - preflt.mag_inconsistency_angle = mag_angle_diff_max; -} - -void VotedSensorsUpdate::update_mag_comp_armed(bool armed) -{ - _mag_compensator.update_armed_flag(armed); -} - -void VotedSensorsUpdate::update_mag_comp_power(float power) -{ - _mag_compensator.update_power(power); -} diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index dd595af23a..faca22d98b 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -39,35 +39,33 @@ * @author Beat Kueng */ -#include "parameters.h" - #include "data_validator/DataValidator.hpp" #include "data_validator/DataValidatorGroup.hpp" -#include +#include #include - #include #include - -#include - #include #include #include +#include +#include #include -#include +#include #include #include #include -#include #include -#include "common.h" - namespace sensors { +static constexpr uint8_t GYRO_COUNT_MAX = 3; +static constexpr uint8_t ACCEL_COUNT_MAX = 3; + +static constexpr uint8_t SENSOR_COUNT_MAX = math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX); + /** ** class VotedSensorsUpdate * @@ -80,8 +78,7 @@ public: * @param parameters parameter values. These do not have to be initialized when constructing this object. * Only when calling init(), they have to be initialized. */ - VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled, - uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]); + VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]); /** * initialize subscriptions etc. @@ -105,7 +102,7 @@ public: /** * read new sensor data */ - void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer); + void sensorsPoll(sensor_combined_s &raw); /** * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, @@ -116,34 +113,22 @@ public: /** * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor */ - void calcAccelInconsistency(sensor_preflight_s &preflt); + void calcAccelInconsistency(sensor_preflight_imu_s &preflt); /** * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor */ - void calcGyroInconsistency(sensor_preflight_s &preflt); - - /** - * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers - */ - void calcMagInconsistency(sensor_preflight_s &preflt); - - /** - * Update armed flag for mag compensation. - */ - void update_mag_comp_armed(bool armed); - - /** - * Update power signal for mag compensation. - * power: either throttle value [0,1] or current measurement in [kA] - */ - void update_mag_comp_power(float power); + void calcGyroInconsistency(sensor_preflight_imu_s &preflt); private: + static constexpr uint8_t ACCEL_COUNT_MAX = 3; + static constexpr uint8_t GYRO_COUNT_MAX = 3; + static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); + struct SensorData { SensorData() = delete; - explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {} + explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {} uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ DataValidatorGroup voter{1}; @@ -151,9 +136,8 @@ private: ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */ uint8_t last_best_vote{0}; /**< index of the latest best vote */ uint8_t subscription_count{0}; - bool enabled[SENSOR_COUNT_MAX] {true, true, true, true}; - bool advertised[SENSOR_COUNT_MAX] {false, false, false, false}; - matrix::Vector3f power_compensation[SENSOR_COUNT_MAX]; + bool enabled[SENSOR_COUNT_MAX] {true, true, true}; + bool advertised[SENSOR_COUNT_MAX] {false, false, false}; }; void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); @@ -166,14 +150,6 @@ private: */ void imuPoll(sensor_combined_s &raw); - /** - * Poll the magnetometer for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void magPoll(vehicle_magnetometer_s &magnetometer); - /** * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) @@ -182,7 +158,6 @@ private: SensorData _accel{ORB_ID::sensor_accel}; SensorData _gyro{ORB_ID::sensor_gyro}; - SensorData _mag{ORB_ID::sensor_mag}; hrt_abstime _last_error_message{0}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -198,26 +173,16 @@ private: }; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ - vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ - matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */ - matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ - - const Parameters &_parameters; const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */ float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ - float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ - - /* Magnetometer interference compensation */ - MagCompensator _mag_compensator; uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */ uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */ - uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */ uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */ diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3f1784773a..8558e2cddb 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -64,12 +64,6 @@ void Simulator::parameters_update(bool force) } } -void Simulator::print_status() -{ - PX4_INFO("magnetometer"); - _px4_mag.print_status(); -} - int Simulator::start(int argc, char *argv[]) { _instance = new Simulator(); @@ -154,7 +148,7 @@ int simulator_main(int argc, char *argv[]) return 1; } else { - Simulator::getInstance()->print_status(); + PX4_INFO("running"); } } else { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 67d863dfb7..61e9d76b01 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -127,8 +127,6 @@ public: bool has_initialized() { return _has_initialized.load(); } #endif - void print_status(); - private: Simulator() : ModuleParams(nullptr) {