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

fw_pos_control_l1: replace sensor_baro with vehicle_air_data

Browse Source 
- controllers shouldn't be accessing raw sensor data directly
This commit is contained in:
Daniel Agar
2020-07-12 11:08:21 -04:00
parent 1394b5d7bc
commit 9a1ca00285
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+5 -5
View File
@@ -1754,13 +1754,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* scale throttle cruise by baro pressure */
if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
sensor_baro_s baro{};
vehicle_air_data_s air_data;
if (_sensor_baro_sub.update(&baro)) {
if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
if (_vehicle_air_data_sub.update(&air_data)) {
if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure);
const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.0f, 1.0f, 2.0f);
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa);
const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f);
throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);