fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 18:59:07 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/CMakeLists.txt
View File

@ -62,3 +62,4 @@ add_subdirectory(terrain_estimation)
add_subdirectory(tunes)
add_subdirectory(version)
add_subdirectory(weather_vane)
add_subdirectory(mag_compensation)

34
src/lib/mag_compensation/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(mag_compensation MagCompensation.cpp)

60
src/lib/mag_compensation/MagCompensation.cpp Normal file
View File

@ -0,0 +1,60 @@
/****************************************************************************
*
* 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 <mathlib/mathlib.h>
MagCompensator::MagCompensator(ModuleParams *parent):
ModuleParams(parent)
{
}
void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect)
{
if (_armed) {
int type = _param_mag_compensation_type.get();
if (type == CompensationType::THROTTLE) {
mag = mag + param_vect * _throttle; //for param [gauss]
} else if (type == CompensationType::CURRENT) {
mag = mag + param_vect * _battery_current * 0.001; //for param [gauss/kA]
}
}
}

78
src/lib/mag_compensation/MagCompensation.hpp Normal file
View File

@ -0,0 +1,78 @@
/****************************************************************************
*
* 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 <roman@auterion.com>
*
* Library for magnetometer data compensation.
*
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
class MagCompensator : public ModuleParams
{
public:
MagCompensator(ModuleParams *parent);
~MagCompensator() = default;
void update_armed_flag(bool armed) { _armed = armed; }
void update_throttle(float throttle) { _throttle = throttle; }
void update_current(float battery_current) { _battery_current = battery_current; }
void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect);
private:
enum CompensationType {
DISABLED = 0,
THROTTLE,
CURRENT
};
float _throttle{0};
float _battery_current{0};
bool _armed{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_compensation_type
)
};

43
src/lib/mag_compensation/mag_compensation_params.c Normal file
View File

@ -0,0 +1,43 @@
/****************************************************************************
*
* 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
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);

1
src/modules/sensors/CMakeLists.txt
View File

@ -57,4 +57,5 @@ px4_add_module(
vehicle_angular_velocity
vehicle_air_data
vehicle_imu
mag_compensation
)

31
src/modules/sensors/parameters.cpp
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);

9
src/modules/sensors/parameters.h
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;

36
src/modules/sensors/sensor_params_mag0.c
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);

36
src/modules/sensors/sensor_params_mag1.c
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);

36
src/modules/sensors/sensor_params_mag2.c
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);

36
src/modules/sensors/sensor_params_mag3.c
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);

15
src/modules/sensors/sensors.cpp
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);
}
}

24
src/modules/sensors/voted_sensors_update.cpp
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);
}

22
src/modules/sensors/voted_sensors_update.h
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 */