diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index e2fd2817b6..519f9db889 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -96,6 +96,17 @@ then gyro_calibration start fi + +if param compare -s MBE_ENABLE 1 +then + # conservative mag bias estimation + param set-default MBE_LEARN_GAIN 5 + param set-default IMU_GYRO_CUTOFF 20 + mag_bias_estimator start +fi + +param set-default SENS_MAG_RATE 100 + sensors start uavcannode start diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index 151aeda7bc..8f69c01983 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 7f8139aceb..480d2c92c5 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 +param set-default MBE_ENABLE 1 safety_button start tone_alarm start diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index 151aeda7bc..8f69c01983 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 4e172c2c3c..25bc347662 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -5,6 +5,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_GPS_RTCM 1 +param set-default MBE_ENABLE 1 safety_button start tone_alarm start diff --git a/boards/cuav/can-gps-v1/default.px4board b/boards/cuav/can-gps-v1/default.px4board index 6a12a5eb40..3a838e1cbe 100644 --- a/boards/cuav/can-gps-v1/default.px4board +++ b/boards/cuav/can-gps-v1/default.px4board @@ -1,6 +1,8 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA" CONFIG_DRIVERS_BAROMETER_MS5611=y @@ -12,5 +14,16 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/freefly/can-rtk-gps/default.px4board b/boards/freefly/can-rtk-gps/default.px4board index 83619d320a..85b1558597 100644 --- a/boards/freefly/can-rtk-gps/default.px4board +++ b/boards/freefly/can-rtk-gps/default.px4board @@ -1,16 +1,22 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y -CONFIG_DRIVERS_IMU_ST=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y -CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y CONFIG_SYSTEMCMDS_MTD=y @@ -19,5 +25,7 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/holybro/can-gps-v1/default.px4board b/boards/holybro/can-gps-v1/default.px4board index 09929ddfa4..60ae33341f 100644 --- a/boards/holybro/can-gps-v1/default.px4board +++ b/boards/holybro/can-gps-v1/default.px4board @@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y @@ -12,6 +13,18 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/matek/gnss-m9n-f4/default.px4board b/boards/matek/gnss-m9n-f4/default.px4board index 55a22c564a..230f1e85a5 100644 --- a/boards/matek/gnss-m9n-f4/default.px4board +++ b/boards/matek/gnss-m9n-f4/default.px4board @@ -1,6 +1,8 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" CONFIG_DRIVERS_BAROMETER_DPS310=y @@ -10,13 +12,20 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y CONFIG_SERIAL_PASSTHRU_UBLOX=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp index 6b48fc099c..6a0e9adc63 100644 --- a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp +++ b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include namespace uavcannode { @@ -51,7 +51,7 @@ class MagneticFieldStrength2 : public: MagneticFieldStrength2(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_magnetometer)), uavcan::Publisher(node) { this->setPriority(uavcan::TransferPriority::Default); @@ -69,15 +69,15 @@ public: void BroadcastAnyUpdates() override { - // sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2 - sensor_mag_s mag; + // vehicle_magnetometer -> uavcan::equipment::ahrs::MagneticFieldStrength2 + vehicle_magnetometer_s vehicle_magnetometer; - if (uORB::SubscriptionCallbackWorkItem::update(&mag)) { + if (uORB::SubscriptionCallbackWorkItem::update(&vehicle_magnetometer)) { uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{}; - magnetic_field.sensor_id = mag.device_id; - magnetic_field.magnetic_field_ga[0] = mag.x; - magnetic_field.magnetic_field_ga[1] = mag.y; - magnetic_field.magnetic_field_ga[2] = mag.z; + magnetic_field.sensor_id = uORB::SubscriptionCallbackWorkItem::get_instance(); + magnetic_field.magnetic_field_ga[0] = vehicle_magnetometer.magnetometer_ga[0]; + magnetic_field.magnetic_field_ga[1] = vehicle_magnetometer.magnetometer_ga[1]; + magnetic_field.magnetic_field_ga[2] = vehicle_magnetometer.magnetometer_ga[2]; uavcan::Publisher::broadcast(magnetic_field); // ensure callback is registered diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index 2b373800ac..6a904137ac 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -116,4 +116,4 @@ PARAM_DEFINE_INT32(SENS_MAG_MODE, 1); * @category system * @group Sensors */ -PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 0); +PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 1); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 48c50365c4..157413c4d5 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -205,6 +205,8 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { + const Vector3f old_offset = _calibration[mag_index].offset(); + // set initial mag calibration const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); @@ -212,6 +214,13 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); + PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]", + mag_index, _calibration[mag_index].device_id(), + (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), + (double)_calibration[mag_index].offset()(0), + (double)_calibration[mag_index].offset()(1), + (double)_calibration[mag_index].offset()(2)); + _calibration_estimator_bias[mag_index].zero(); parameters_notify = true;