dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

support mag power compensation with battery_status instance 1

This commit is contained in:
baumanta 2020-04-21 16:37:23 +02:00 committed by Beat Küng
parent 503bd15b82
commit 48cf38d623
8 changed files with 51 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/mag_compensation/MagCompensation.cpp
View File

@ -48,13 +48,6 @@ MagCompensator::MagCompensator(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]
}
mag = mag + param_vect * _power;
}
}

19
src/lib/mag_compensation/MagCompensation.hpp
View File

@ -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 &param_vect);
private:
enum CompensationType {
DISABLED = 0,
THROTTLE,
CURRENT
};
float _throttle{0};
float _battery_current{0};
float _power{0};
bool _armed{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_compensation_type
)
};

3
src/lib/mag_compensation/mag_compensation_params.c
View File

@ -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

4
src/modules/sensors/parameters.cpp
View File

@ -59,6 +59,8 @@ void initialize_parameter_handles(ParameterHandles &parameter_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 &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_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]));

2
src/modules/sensors/parameters.h
View File

@ -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];

43
src/modules/sensors/sensors.cpp
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]
}
}
}

9
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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);
}

10
src/modules/sensors/voted_sensors_update.h
View File

@ -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: