From 22ceeccc26e842e8edd0fb93a70026b26d65f400 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 24 Mar 2020 09:56:31 +0100 Subject: [PATCH] add library for current- or thrust-based mag compensation --- src/lib/CMakeLists.txt | 1 + src/lib/mag_compensation/CMakeLists.txt | 34 ++++++++ src/lib/mag_compensation/MagCompensation.cpp | 60 ++++++++++++++ src/lib/mag_compensation/MagCompensation.hpp | 78 +++++++++++++++++++ .../mag_compensation_params.c | 43 ++++++++++ src/modules/sensors/CMakeLists.txt | 1 + src/modules/sensors/parameters.cpp | 31 ++++++++ src/modules/sensors/parameters.h | 9 +++ src/modules/sensors/sensor_params_mag0.c | 36 +++++++++ src/modules/sensors/sensor_params_mag1.c | 36 +++++++++ src/modules/sensors/sensor_params_mag2.c | 36 +++++++++ src/modules/sensors/sensor_params_mag3.c | 36 +++++++++ src/modules/sensors/sensors.cpp | 15 ++++ src/modules/sensors/voted_sensors_update.cpp | 24 +++++- src/modules/sensors/voted_sensors_update.h | 22 +++++- 15 files changed, 460 insertions(+), 2 deletions(-) create mode 100644 src/lib/mag_compensation/CMakeLists.txt create mode 100644 src/lib/mag_compensation/MagCompensation.cpp create mode 100644 src/lib/mag_compensation/MagCompensation.hpp create mode 100644 src/lib/mag_compensation/mag_compensation_params.c diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index e4321183b2..107975062d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -62,3 +62,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) +add_subdirectory(mag_compensation) diff --git a/src/lib/mag_compensation/CMakeLists.txt b/src/lib/mag_compensation/CMakeLists.txt new file mode 100644 index 0000000000..aee68a89c9 --- /dev/null +++ b/src/lib/mag_compensation/CMakeLists.txt @@ -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) diff --git a/src/lib/mag_compensation/MagCompensation.cpp b/src/lib/mag_compensation/MagCompensation.cpp new file mode 100644 index 0000000000..cd923a0924 --- /dev/null +++ b/src/lib/mag_compensation/MagCompensation.cpp @@ -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 + +MagCompensator::MagCompensator(ModuleParams *parent): + ModuleParams(parent) +{ + +} + +void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f ¶m_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] + } + } +} diff --git a/src/lib/mag_compensation/MagCompensation.hpp b/src/lib/mag_compensation/MagCompensation.hpp new file mode 100644 index 0000000000..dd649f85a5 --- /dev/null +++ b/src/lib/mag_compensation/MagCompensation.hpp @@ -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 + * + * Library for magnetometer data compensation. + * + */ + +#pragma once + +#include +#include + +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 ¶m_vect); + +private: + + enum CompensationType { + DISABLED = 0, + THROTTLE, + CURRENT + }; + + float _throttle{0}; + float _battery_current{0}; + bool _armed{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mag_compensation_type + ) + +}; diff --git a/src/lib/mag_compensation/mag_compensation_params.c b/src/lib/mag_compensation/mag_compensation_params.c new file mode 100644 index 0000000000..527ee5a5c3 --- /dev/null +++ b/src/lib/mag_compensation/mag_compensation_params.c @@ -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); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0b68a2c23d..e4ea12c319 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -57,4 +57,5 @@ px4_add_module( vehicle_angular_velocity vehicle_air_data vehicle_imu + mag_compensation ) diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 7d29a3035d..182ba3ba25 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -58,6 +58,22 @@ void initialize_parameter_handles(ParameterHandles ¶meter_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 ¶meter_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, ¶meters.air_cmodel); param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index f2870e3c2a..42ea41a329 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -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; diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index 59b1618066..5420df54ca 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/src/modules/sensors/sensor_params_mag0.c @@ -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); diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index 5d797390dc..cfb2ea0f33 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/src/modules/sensors/sensor_params_mag1.c @@ -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); diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index 2eebb36508..d543f12bde 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/src/modules/sensors/sensor_params_mag2.c @@ -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); diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index 977c70283a..bd29b3511a 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/src/modules/sensors/sensor_params_mag3.c @@ -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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a4c5ac4b6f..d52417aa85 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #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_pub{ORB_ID(airspeed)}; /**< airspeed */ uORB::Publication _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); } } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 516b862898..803ce68c4f 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -52,7 +52,7 @@ using namespace sensors; using namespace matrix; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, 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); +} diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 53d00a490e..3b4e1177b9 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -51,6 +51,7 @@ #include #include +#include #include #include @@ -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 */