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

TECS: added method to get SKE weighting 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-09-22 11:49:34 +03:00 committed by Roman Bapst
parent d0e8b882a2
commit 8823f5e1ec
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/lib/tecs/TECS.cpp
View File

@ -398,18 +398,10 @@ void TECS::_update_pitch_setpoint()
* The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
*/
// Calculate the weighting applied to control of specific kinetic energy error
float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
SKE_weighting = 2.0f;
} else if (!airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
}
const float SKE_weighting = get_SKE_weighting();
// Calculate the weighting applied to control of specific potential energy error
float SPE_weighting = 2.0f - SKE_weighting;
const float SPE_weighting = 2.0f - SKE_weighting;
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
@ -617,3 +609,18 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float
}
}
float TECS::get_SKE_weighting()
{
// Calculate the weighting applied to control of specific kinetic energy error
float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
SKE_weighting = 2.0f;
} else if (!airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
}
return SKE_weighting;
}

2
src/lib/tecs/TECS.hpp
View File

@ -331,6 +331,8 @@ private:
*/
void _update_STE_rate_lim();
float get_SKE_weighting();
float _param_filt_speed_deriv{0.95f};
AlphaFilter<float> _STE_rate_error_filter;