mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
Plane: fixed an issue with landing on rising ground
when landing on rising ground we don't want to use the terrain look fwd correction for the flare calculation as otherwise we will flare too early (and thus too high)
This commit is contained in:
parent
929426abf8
commit
b8d3640bc4
@ -559,6 +559,9 @@ private:
|
|||||||
|
|
||||||
// are we doing loiter mode as a VTOL?
|
// are we doing loiter mode as a VTOL?
|
||||||
bool vtol_loiter;
|
bool vtol_loiter;
|
||||||
|
|
||||||
|
// how much correction have we added for terrain data
|
||||||
|
float terrain_correction;
|
||||||
} auto_state;
|
} auto_state;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
|
@ -595,6 +595,7 @@ void Plane::rangefinder_terrain_correction(float &height)
|
|||||||
}
|
}
|
||||||
float correction = (terrain_amsl1 - terrain_amsl2);
|
float correction = (terrain_amsl1 - terrain_amsl2);
|
||||||
height += correction;
|
height += correction;
|
||||||
|
auto_state.terrain_correction = correction;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -230,7 +230,11 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
// use rangefinder to correct if possible
|
// use rangefinder to correct if possible
|
||||||
const float height = height_above_target() - rangefinder_correction();
|
float height = height_above_target() - rangefinder_correction();
|
||||||
|
// for flare calculations we don't want to use the terrain
|
||||||
|
// correction as otherwise we will flare early on rising
|
||||||
|
// ground
|
||||||
|
height -= auto_state.terrain_correction;
|
||||||
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
|
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
|
||||||
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range);
|
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user