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

AP_Landing: Internalize the flight mode slope stages

This commit is contained in:
Michael du Breuil 2017-01-10 12:46:51 -07:00 committed by Tom Pittenger
parent b570c11e26
commit e9ec1d3a30
3 changed files with 60 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Landing/AP_Landing.cpp
View File

@ -264,7 +264,6 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo
void AP_Landing::reset(void)
{
complete = false;
pre_flare = false;
commanded_go_around = false;
initial_slope = 0;
slope = 0;
@ -355,46 +354,15 @@ float AP_Landing::head_wind(void)
/*
* returns target airspeed in cm/s depending on flight stage
*/
int32_t AP_Landing::get_target_airspeed_cm(const AP_Vehicle::FixedWing::FlightStage flight_stage)
int32_t AP_Landing::get_target_airspeed_cm(void)
{
int32_t target_airspeed_cm = aparm.airspeed_cruise_cm;
if (!in_progress) {
// not landing, use regular cruise airspeed
return target_airspeed_cm;
}
// we're landing, check for custom approach and
// pre-flare airspeeds. Also increase for head-winds
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
switch (flight_stage) {
case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH:
if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE:
case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL:
if (pre_flare && get_pre_flare_airspeed() > 0) {
// if we just preflared then continue using the pre-flare airspeed during final flare
target_airspeed_cm = get_pre_flare_airspeed() * 100;
} else if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_get_target_airspeed_cm();
default:
break;
return SpdHgt_Controller->get_land_airspeed();
}
// when landing, add half of head-wind.
const int32_t head_wind_compensation_cm = head_wind() * 0.5f * 100;
// Do not lower it or exceed cruise speed
return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, aparm.airspeed_cruise_cm);
}
/*

14
libraries/AP_Landing/AP_Landing.h
View File

@ -85,13 +85,12 @@ public:
bool restart_landing_sequence(void);
float wind_alignment(const float heading_deg);
float head_wind(void);
int32_t get_target_airspeed_cm(const AP_Vehicle::FixedWing::FlightStage flight_stage);
int32_t get_target_airspeed_cm(void);
// accessor functions for the params and states
static const struct AP_Param::GroupInfo var_info[];
int16_t get_pitch_cd(void) const { return pitch_cd; }
float get_flare_sec(void) const { return flare_sec; }
float get_pre_flare_airspeed(void) const { return pre_flare_airspeed; }
int8_t get_disarm_delay(void) const { return disarm_delay; }
int8_t get_then_servos_neutral(void) const { return then_servos_neutral; }
int8_t get_abort_throttle_enable(void) const { return abort_throttle_enable; }
@ -102,15 +101,10 @@ public:
void set_initial_slope(void) { initial_slope = slope; }
bool is_expecting_impact(void) const;
AP_Vehicle::FixedWing::FlightStage stage;
// Flag to indicate if we have landed.
// Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
bool complete;
// Flag to indicate if we have triggered pre-flare. This occurs when we have reached LAND_PF_ALT
bool pre_flare;
// are we in auto and flight mode is approach || pre-flare || final (flare)
bool in_progress;
@ -163,6 +157,10 @@ private:
AP_Int8 type;
// Land Type STANDARD GLIDE SLOPE
enum slope_stage {SLOPE_APPROACH,
SLOPE_PREFLARE,
SLOPE_FINAL};
slope_stage type_slope_stage;
void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed);
bool type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range);
@ -170,11 +168,11 @@ private:
void type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm);
void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm);
int32_t type_slope_get_target_airspeed_cm(void);
void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
bool type_slope_request_go_around(void);
bool type_slope_is_flaring(void) const;
bool type_slope_is_on_approach(void) const;
bool type_slope_is_expecting_impact(void) const;
};

59
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -27,7 +27,6 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo
// the altitude has been reached, restart the landing sequence
throttle_suppressed = false;
complete = false;
pre_flare = false;
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
}
@ -80,7 +79,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
(double)get_distance(current_loc, next_WP_loc));
}
complete = true;
update_flight_stage_fn();
type_slope_stage = SLOPE_FINAL;
}
@ -93,12 +92,11 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
aparm.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
} else if (!complete && !pre_flare && pre_flare_airspeed > 0) {
} else if (!complete && type_slope_stage != SLOPE_PREFLARE && pre_flare_airspeed > 0) {
bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt);
bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec);
if (reached_pre_flare_alt || reached_pre_flare_sec) {
pre_flare = true;
update_flight_stage_fn();
type_slope_stage = SLOPE_PREFLARE;
}
}
@ -284,20 +282,61 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
constrain_target_altitude_location_fn(loc, prev_WP_loc);
}
int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) {
int32_t target_airspeed_cm = aparm.airspeed_cruise_cm;
if (!in_progress) {
// not landing, use regular cruise airspeed
return target_airspeed_cm;
}
// we're landing, check for custom approach and
// pre-flare airspeeds. Also increase for head-winds
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
switch (type_slope_stage) {
case SLOPE_APPROACH:
if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
case SLOPE_PREFLARE:
case SLOPE_FINAL:
if (pre_flare_airspeed > 0) {
// if we just preflared then continue using the pre-flare airspeed during final flare
target_airspeed_cm = pre_flare_airspeed * 100;
} else if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
default:
break;
}
// when landing, add half of head-wind.
const int32_t head_wind_compensation_cm = head_wind() * 0.5f * 100;
// Do not lower it or exceed cruise speed
return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, aparm.airspeed_cruise_cm);
}
bool AP_Landing::type_slope_is_flaring(void) const
{
return (stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_LAND_FINAL);
return type_slope_stage == SLOPE_FINAL;
}
bool AP_Landing::type_slope_is_on_approach(void) const
{
return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH ||
stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
return (type_slope_stage == SLOPE_APPROACH ||
type_slope_stage == SLOPE_PREFLARE);
}
bool AP_Landing::type_slope_is_expecting_impact(void) const
{
return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE ||
stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL);
return (type_slope_stage == SLOPE_PREFLARE ||
type_slope_stage == SLOPE_FINAL);
}