mirror of https://github.com/ArduPilot/ardupilot
Plane: separate out landing height adjustment from barometer changes
this stores a landing height adjustment for an aborted landing without adjusting barometer readings, applying them only on landing
This commit is contained in:
parent
2b1b23cabf
commit
4da3236c07
|
@ -485,8 +485,8 @@ void Plane::update_GPS_10Hz(void)
|
|||
if (!hal.util->get_soft_armed()) {
|
||||
update_home();
|
||||
|
||||
// zero out any baro drift
|
||||
barometer.set_baro_drift_altitude(0);
|
||||
// reset the landing altitude correction
|
||||
auto_state.land_alt_offset = 0;
|
||||
}
|
||||
|
||||
// update wind estimate
|
||||
|
|
|
@ -524,6 +524,9 @@ private:
|
|||
|
||||
// are we doing loiter mode as a VTOL?
|
||||
bool vtol_loiter:1;
|
||||
|
||||
// landing altitude offset (meters)
|
||||
float land_alt_offset;
|
||||
} auto_state;
|
||||
|
||||
struct {
|
||||
|
@ -856,6 +859,7 @@ private:
|
|||
void setup_terrain_target_alt(Location &loc);
|
||||
int32_t adjusted_altitude_cm(void);
|
||||
int32_t adjusted_relative_altitude_cm(void);
|
||||
float mission_alt_offset(void);
|
||||
float height_above_target(void);
|
||||
float lookahead_adjustment(void);
|
||||
float rangefinder_correction(void);
|
||||
|
|
|
@ -249,7 +249,7 @@ int32_t Plane::relative_target_altitude_cm(void)
|
|||
}
|
||||
#endif
|
||||
int32_t relative_alt = target_altitude.amsl_cm - home.alt;
|
||||
relative_alt += int32_t(g.alt_offset)*100;
|
||||
relative_alt += mission_alt_offset()*100;
|
||||
relative_alt += rangefinder_correction() * 100;
|
||||
return relative_alt;
|
||||
}
|
||||
|
@ -451,7 +451,7 @@ void Plane::setup_terrain_target_alt(Location &loc)
|
|||
*/
|
||||
int32_t Plane::adjusted_altitude_cm(void)
|
||||
{
|
||||
return current_loc.alt - (g.alt_offset*100);
|
||||
return current_loc.alt - (mission_alt_offset()*100);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -464,6 +464,24 @@ int32_t Plane::adjusted_relative_altitude_cm(void)
|
|||
return adjusted_altitude_cm() - home.alt;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
return the mission altitude offset. This raises or lowers all
|
||||
mission items. It is primarily set using the ALT_OFFSET parameter,
|
||||
but can also be adjusted by the rangefinder landing code for a
|
||||
NAV_LAND command if we have aborted a steep landing
|
||||
*/
|
||||
float Plane::mission_alt_offset(void)
|
||||
{
|
||||
float ret = g.alt_offset;
|
||||
if (control_mode == AUTO && auto_state.land_in_progress) {
|
||||
// when landing after an aborted landing due to too high glide
|
||||
// slope we use an offset from the last landing attempt
|
||||
ret += auto_state.land_alt_offset;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
return the height in meters above the next_WP_loc altitude
|
||||
*/
|
||||
|
|
|
@ -200,8 +200,10 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
|
|||
|
||||
// is projected slope too steep?
|
||||
if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) {
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Slope re-calculated too steep, abort landing!");
|
||||
barometer.set_baro_drift_altitude(barometer.get_baro_drift_offset() - rangefinder_state.correction);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
|
||||
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
|
||||
auto_state.land_alt_offset = rangefinder_state.correction;
|
||||
auto_state.commanded_go_around = 1;
|
||||
g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue