fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 18:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add mag bias estimator to CAN GPS units with IMUs

Browse Source 
- 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-19 15:46:36 -04:00
committed by Daniel Agar
parent 73f45fee6e
commit 4cc3e78558
12 changed files with 87 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp
+9 -9
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
src/modules/sensors/sensor_params_mag.c
+1 -1
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);
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+9
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;