Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: fixed adjusting speed without airspeed sensor 

the pitch trim variable that was not connected in aparm is needed to
allow tuning of the flight speed using PTCH_TRIM_DEG and
TRIM_THROTTLE.

This was broken in 4.4.x by this PR:

https://github.com/ArduPilot/ardupilot/pull/22191
This commit is contained in:
Andrew Tridgell 2024-03-03 08:25:06 +11:00 committed by Peter Barker
parent c7a4a37398
commit 8851a92dfd
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_TECS/AP_TECS.cpp
View File

@ -853,7 +853,7 @@ float AP_TECS::_get_i_gain(void)
/* /*
calculate throttle, non-airspeed case calculate throttle, non-airspeed case
*/ */
void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg)
{ {
// reset clip status after possible use of synthetic airspeed // reset clip status after possible use of synthetic airspeed
_thr_clip_status = clipStatus::NONE; _thr_clip_status = clipStatus::NONE;
@ -873,7 +873,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
_pitch_demand_lpf.apply(_pitch_dem, _DT); _pitch_demand_lpf.apply(_pitch_dem, _DT);
const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get();
_pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT);
const float pitch_corrected_lpf = _pitch_measured_lpf.get(); const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(pitch_trim_deg);
const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf;
if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f) if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f)
@ -1205,7 +1205,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge, int16_t throttle_nudge,
float hgt_afe, float hgt_afe,
float load_factor) float load_factor,
float pitch_trim_deg)
{ {
uint64_t now = AP_HAL::micros64(); uint64_t now = AP_HAL::micros64();
// check how long since we last did the 50Hz update; do nothing in // check how long since we last did the 50Hz update; do nothing in
@ -1379,7 +1380,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_use_synthetic_airspeed_once = false; _use_synthetic_airspeed_once = false;
_using_airspeed_for_throttle = true; _using_airspeed_for_throttle = true;
} else { } else {
_update_throttle_without_airspeed(throttle_nudge); _update_throttle_without_airspeed(throttle_nudge, pitch_trim_deg);
_using_airspeed_for_throttle = false; _using_airspeed_for_throttle = false;
} }

5
libraries/AP_TECS/AP_TECS.h
View File

@ -52,7 +52,8 @@ public:
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge, int16_t throttle_nudge,
float hgt_afe, float hgt_afe,
float load_factor); float load_factor,
float pitch_trim_deg);
// demanded throttle in percentage // demanded throttle in percentage
// should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0 // should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0
@ -455,7 +456,7 @@ private:
void _update_throttle_with_airspeed(void); void _update_throttle_with_airspeed(void);
// Update Demanded Throttle Non-Airspeed // Update Demanded Throttle Non-Airspeed
void _update_throttle_without_airspeed(int16_t throttle_nudge); void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg);
// get integral gain which is flight_stage dependent // get integral gain which is flight_stage dependent
float _get_i_gain(void); float _get_i_gain(void);