fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 17:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

add library for current- or thrust-based mag compensation

Browse Source
This commit is contained in:
baumanta
2020-03-24 09:56:31 +01:00
committed by Beat Küng
parent 44bec269b0
commit 22ceeccc26
15 changed files with 460 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/CMakeLists.txt
+1
View File
@@ -57,4 +57,5 @@ px4_add_module(
vehicle_angular_velocity
vehicle_air_data
vehicle_imu
mag_compensation
)
src/modules/sensors/parameters.cpp
+31
View File
@@ -58,6 +58,22 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
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_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");
@@ -104,6 +120,21 @@ void update_parameters(const ParameterHandles &parameter_handles, Parameters &pa
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_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, &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);
src/modules/sensors/parameters.h
+9
View File
@@ -56,6 +56,11 @@ struct Parameters {
float board_offset[3];
//parameters for current/throttle-based mag compensation
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;
@@ -71,6 +76,10 @@ struct ParameterHandles {
param_t board_offset[3];
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;
src/modules/sensors/sensor_params_mag0.c
+36
View File
@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_XCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Y component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Z component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0f);
src/modules/sensors/sensor_params_mag1.c
+36
View File
@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_XCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Y component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Z component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0f);
src/modules/sensors/sensor_params_mag2.c
+36
View File
@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_XCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Y component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Z component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0f);
src/modules/sensors/sensor_params_mag3.c
+36
View File
@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f);
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f);
/**
* Coefficient describing linear relationship between
* X component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_XCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Y component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0f);
/**
* Coefficient describing linear relationship between
* Z component of magnetometer in body frame axis
* and either current or throttle depending on value of CAL_MAG_COMP_TYP
* Unit for throttle-based compensation is [G] and
* for current-based compensation [G/kA]
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0f);
src/modules/sensors/sensors.cpp
+15
View File
@@ -69,6 +69,7 @@
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/battery_status.h>
#include "parameters.h"
#include "voted_sensors_update.h"
@@ -124,6 +125,7 @@ private:
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)}; /**< vehicle control mode subscription */
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */
@@ -464,6 +466,19 @@ void Sensors::Run()
if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
_armed = vcontrol_mode.flag_armed;
_voted_sensors_update.update_mag_comp_armed(_armed);
}
if (_actuator_ctrl_0_sub.updated()) {
actuator_controls_s controls {};
_actuator_ctrl_0_sub.copy(&controls);
_voted_sensors_update.update_mag_comp_throttle(controls.control[actuator_controls_s::INDEX_THROTTLE]);
}
if (_battery_status_sub.updated()) {
battery_status_s bat_stat {};
_battery_status_sub.copy(&bat_stat);
_voted_sensors_update.update_mag_comp_current(bat_stat.current_a);
}
}
src/modules/sensors/voted_sensors_update.cpp
+23 -1
View File
@@ -52,7 +52,7 @@ using namespace sensors;
using namespace matrix;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: _parameters(parameters), _hil_enabled(hil_enabled)
: ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this)
{
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale_0[i] = 1.0f;
@@ -121,6 +121,8 @@ void VotedSensorsUpdate::parametersUpdate()
_mag_rotation[topic_instance] = _board_rotation;
}
updateParams();
/* set offset parameters to new values */
bool failed = false;
@@ -397,6 +399,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* 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
@@ -679,6 +682,10 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
}
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;
@@ -980,3 +987,18 @@ void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
// 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_throttle(float throttle)
{
_mag_compensator.update_throttle(throttle);
}
void VotedSensorsUpdate::update_mag_comp_current(float current)
{
_mag_compensator.update_current(current);
}
src/modules/sensors/voted_sensors_update.h
+21 -1
View File
@@ -51,6 +51,7 @@
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <lib/mag_compensation/MagCompensation.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
@@ -74,7 +75,7 @@ namespace sensors
*
* Handling of sensor updates with voting
*/
class VotedSensorsUpdate
class VotedSensorsUpdate : public ModuleParams
{
public:
/**
@@ -140,6 +141,21 @@ public:
*/
void calcMagInconsistency(sensor_preflight_s &preflt);
/**
* Update armed flag for mag compensation.
*/
void update_mag_comp_armed(bool armed);
/**
* Update throttle for mag compensation.
*/
void update_mag_comp_throttle(float throttle);
/**
* Update current for mag compensation.
*/
void update_mag_comp_current(float current);
private:
struct SensorData {
@@ -164,6 +180,7 @@ private:
int subscription_count;
DataValidatorGroup voter;
unsigned int last_failover_count;
matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
};
void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max);
@@ -225,6 +242,9 @@ private:
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 */