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

support mag power compensation with battery_status instance 1

Browse Source
This commit is contained in:
baumanta
2020-04-28 08:36:44 +02:00
committed by Beat Küng
parent 503bd15b82
commit 48cf38d623
8 changed files with 51 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+35 -8
View File
@@ -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_s> _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */
uORB::Publication<sensor_combined_s> _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]
}
}
}