Skip to content

Commit

Permalink
Plane: fixed bug in pullup code
Browse files Browse the repository at this point in the history
if we have poor pitch trim it is possible we will pullup before
reaching the target airspeed. Check pitch threshold during airspeed
stage of pullup
  • Loading branch information
tridge committed Sep 29, 2024
1 parent ecec9c4 commit b606e77
Showing 1 changed file with 1 addition and 1 deletion.
2 changes: 1 addition & 1 deletion ArduPlane/pullup.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ bool GliderPullup::verify_pullup(void)
switch (stage) {
case Stage::WAIT_AIRSPEED: {
float aspeed;
if (ahrs.airspeed_estimate(aspeed) && aspeed > airspeed_start) {
if (ahrs.airspeed_estimate(aspeed) && (aspeed > airspeed_start || ahrs.pitch_sensor*0.01 > pitch_start)) {
gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01);
stage = Stage::WAIT_PITCH;
}
Expand Down

0 comments on commit b606e77

Please sign in to comment.