From 48cf38d623482126535d3311a790d11737709fb7 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 21 Apr 2020 16:37:23 +0200 Subject: [PATCH] support mag power compensation with battery_status instance 1 --- src/lib/mag_compensation/MagCompensation.cpp | 9 +--- src/lib/mag_compensation/MagCompensation.hpp | 19 +------- .../mag_compensation_params.c | 3 +- src/modules/sensors/parameters.cpp | 4 ++ src/modules/sensors/parameters.h | 2 + src/modules/sensors/sensors.cpp | 43 +++++++++++++++---- src/modules/sensors/voted_sensors_update.cpp | 9 +--- src/modules/sensors/voted_sensors_update.h | 10 ++--- 8 files changed, 51 insertions(+), 48 deletions(-) diff --git a/src/lib/mag_compensation/MagCompensation.cpp b/src/lib/mag_compensation/MagCompensation.cpp index cd923a0924..6903b9384a 100644 --- a/src/lib/mag_compensation/MagCompensation.cpp +++ b/src/lib/mag_compensation/MagCompensation.cpp @@ -48,13 +48,6 @@ MagCompensator::MagCompensator(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] - } + mag = mag + param_vect * _power; } } diff --git a/src/lib/mag_compensation/MagCompensation.hpp b/src/lib/mag_compensation/MagCompensation.hpp index dd649f85a5..552d0a482f 100644 --- a/src/lib/mag_compensation/MagCompensation.hpp +++ b/src/lib/mag_compensation/MagCompensation.hpp @@ -53,26 +53,11 @@ public: 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 update_power(float power) { _power = power; } 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}; + float _power{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 index 527ee5a5c3..8ec9b72983 100644 --- a/src/lib/mag_compensation/mag_compensation_params.c +++ b/src/lib/mag_compensation/mag_compensation_params.c @@ -35,7 +35,8 @@ * * @value 0 Disabled * @value 1 Throttle-based compensation - * @value 2 Current-based compensation + * @value 2 Current-based compensation (battery_status instance 0) + * @value 3 Current-based compensation (battery_status instance 1) * * @category system * @group Sensor Calibration diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 182ba3ba25..aa0f187977 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -59,6 +59,8 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* mag compensation */ + parameter_handles.mag_comp_type = param_find("CAL_MAG_COMP_TYP"); + 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"); @@ -120,6 +122,8 @@ 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_type, &(parameters.mag_comp_type)); + 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])); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 42ea41a329..f4a86a76a3 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -57,6 +57,7 @@ struct Parameters { float board_offset[3]; //parameters for current/throttle-based mag compensation + int32_t mag_comp_type; float mag_comp_paramX[4]; float mag_comp_paramY[4]; float mag_comp_paramZ[4]; @@ -76,6 +77,7 @@ struct ParameterHandles { param_t board_offset[3]; + param_t mag_comp_type; param_t mag_comp_paramX[4]; param_t mag_comp_paramY[4]; param_t mag_comp_paramZ[4]; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b675f47d0f..998ea9ce0c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -125,7 +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::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; /**< battery_status instance 0 subscription */ uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */ @@ -138,6 +138,15 @@ private: {this, ORB_ID(sensor_gyro_integrated), 2} }; + enum class MagCompensationType { + Disabled = 0, + Throttle, + Current_inst0, + Current_inst1 + }; + + MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; + uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor_sub_index{0}; @@ -468,16 +477,34 @@ void Sensors::Run() _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]); + //check mag power compensation type (change battery current subscription instance if necessary) + if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0 + && _mag_comp_type != MagCompensationType::Current_inst0) { + _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0}; } - if (_battery_status_sub.updated()) { + if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1 + && _mag_comp_type != MagCompensationType::Current_inst1) { + _battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1}; + } + + _mag_comp_type = (MagCompensationType)_parameters.mag_comp_type; + + //update power signal for mag compensation + if (_mag_comp_type == MagCompensationType::Throttle) { + actuator_controls_s controls {}; + + if (_actuator_ctrl_0_sub.update(&controls)) { + _voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]); + } + + } else if (_mag_comp_type == MagCompensationType::Current_inst0 + || _mag_comp_type == MagCompensationType::Current_inst1) { battery_status_s bat_stat {}; - _battery_status_sub.copy(&bat_stat); - _voted_sensors_update.update_mag_comp_current(bat_stat.current_a); + + if (_battery_status_sub.update(&bat_stat)) { + _voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA] + } } } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d920e18820..ff7950d3fb 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -954,12 +954,7 @@ void VotedSensorsUpdate::update_mag_comp_armed(bool armed) _mag_compensator.update_armed_flag(armed); } -void VotedSensorsUpdate::update_mag_comp_throttle(float throttle) +void VotedSensorsUpdate::update_mag_comp_power(float power) { - _mag_compensator.update_throttle(throttle); -} - -void VotedSensorsUpdate::update_mag_comp_current(float current) -{ - _mag_compensator.update_current(current); + _mag_compensator.update_power(power); } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index ab89386b9e..782ca94ef8 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -142,14 +142,10 @@ public: void update_mag_comp_armed(bool armed); /** - * Update throttle for mag compensation. + * Update power signal for mag compensation. + * power: either throttle value [0,1] or current measurement in [kA] */ - void update_mag_comp_throttle(float throttle); - - /** - * Update current for mag compensation. - */ - void update_mag_comp_current(float current); + void update_mag_comp_power(float power); private: