dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add mag bias estimator to CAN GPS units with IMUs 

- run `sensors` hub to process sensor_mag and publish vehicle_magnetometer
 - update uavcannode to use vehicle_magnetometer
This commit is contained in:
alexklimaj 2022-06-17 18:36:55 -06:00 committed by Daniel Agar
parent 73f45fee6e
commit 4cc3e78558
12 changed files with 87 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ROMFS/cannode/init.d/rcS
View File

@ -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

5
boards/ark/can-gps/default.px4board
View File

@ -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

1
boards/ark/can-gps/init/rc.board_defaults
View File

@ -4,6 +4,7 @@
#------------------------------------------------------------------------------
param set-default CBRK_IO_SAFETY 0
param set-default MBE_ENABLE 1
safety_button start
tone_alarm start

5
boards/ark/can-rtk-gps/default.px4board
View File

@ -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

1
boards/ark/can-rtk-gps/init/rc.board_defaults
View File

@ -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

13
boards/cuav/can-gps-v1/default.px4board
View File

@ -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

12
boards/freefly/can-rtk-gps/default.px4board
View File

@ -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

13
boards/holybro/can-gps-v1/default.px4board
View File

@ -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

9
boards/matek/gnss-m9n-f4/default.px4board
View File

@ -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

18
src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp
View File

@ -38,7 +38,7 @@
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_magnetometer.h>
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<uavcan::equipment::ahrs::MagneticFieldStrength2>(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<uavcan::equipment::ahrs::MagneticFieldStrength2>::broadcast(magnetic_field);
// ensure callback is registered

2
src/modules/sensors/sensor_params_mag.c
View File

@ -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);

9
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -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;