Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 23:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: rename stage enum

Browse Source
This commit is contained in:
Tom Pittenger 2017-01-10 22:18:09 -08:00
parent 650e694eb1
commit 2ccd91d88f
4 changed files with 56 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -865,7 +865,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
return;
}
landing.set_in_progress(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",

23
libraries/AP_Landing/AP_Landing.cpp
View File

@ -217,6 +217,10 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
// return true while the aircraft should be in a flaring state
bool AP_Landing::is_flaring(void) const
{
if (!in_progress) {
return false;
}
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_flaring();
@ -229,6 +233,10 @@ bool AP_Landing::is_flaring(void) const
// return true while the aircraft is performing a landing approach
bool AP_Landing::is_on_approach(void) const
{
if (!in_progress) {
return false;
}
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_on_approach();
@ -241,6 +249,10 @@ bool AP_Landing::is_on_approach(void) const
// return true when at the last stages of a land when an impact with the ground is expected soon
bool AP_Landing::is_expecting_impact(void) const
{
if (!in_progress) {
return false;
}
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_expecting_impact();
@ -356,6 +368,11 @@ float AP_Landing::head_wind(void)
*/
int32_t AP_Landing::get_target_airspeed_cm(void)
{
if (!in_progress) {
// not landing, use regular cruise airspeed
return aparm.airspeed_cruise_cm;
}
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_get_target_airspeed_cm();
@ -380,4 +397,10 @@ bool AP_Landing::request_go_around(void)
}
}
void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage)
{
in_progress = _in_landing_stage;
commanded_go_around = false;
}

9
libraries/AP_Landing/AP_Landing.h
View File

@ -78,7 +78,7 @@ public:
bool request_go_around(void);
bool is_flaring(void) const;
bool is_on_approach(void) const;
void set_in_progress(const bool _in_progress) { in_progress = _in_progress; }
void handle_flight_stage_change(const bool _in_landing_stage);
// helper functions
bool restart_landing_sequence(void);
@ -157,9 +157,10 @@ private:
// Land Type STANDARD GLIDE SLOPE
enum slope_stage {
SLOPE_APPROACH,
SLOPE_PREFLARE,
SLOPE_FINAL
SLOPE_STAGE_NORMAL,
SLOPE_STAGE_APPROACH,
SLOPE_STAGE_PREFLARE,
SLOPE_STAGE_FINAL
};
slope_stage type_slope_stage;
void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);

48
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -29,7 +29,7 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons
// once landed, post some landing statistics to the GCS
post_stats = false;
type_slope_stage = SLOPE_APPROACH;
type_slope_stage = SLOPE_STAGE_NORMAL;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
}
@ -50,11 +50,24 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo
bool AP_Landing::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)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Stage: %d Complete: %d", type_slope_stage, complete);
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
// determine stage
if (type_slope_stage == SLOPE_STAGE_NORMAL) {
const bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
const bool on_flight_line = fabsf(nav_controller->crosstrack_error()) < 5.0f && !nav_controller->data_is_stale();
const bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((mission.get_prev_nav_cmd_id() == MAV_CMD_NAV_LOITER_TO_ALT) ||
(wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
(wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
(wp_proportion > 0.5f)) {
type_slope_stage = SLOPE_STAGE_APPROACH;
}
}
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
@ -92,7 +105,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;
type_slope_stage = SLOPE_FINAL;
type_slope_stage = SLOPE_STAGE_FINAL;
}
@ -105,11 +118,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 && type_slope_stage != SLOPE_PREFLARE && pre_flare_airspeed > 0) {
} else if (!complete && type_slope_stage == SLOPE_STAGE_APPROACH && 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) {
type_slope_stage = SLOPE_PREFLARE;
type_slope_stage = SLOPE_STAGE_PREFLARE;
}
}
@ -296,27 +309,22 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
}
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();
int32_t target_airspeed_cm = aparm.airspeed_cruise_cm;
switch (type_slope_stage) {
case SLOPE_APPROACH:
case SLOPE_STAGE_APPROACH:
if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
case SLOPE_PREFLARE:
case SLOPE_FINAL:
case SLOPE_STAGE_PREFLARE:
case SLOPE_STAGE_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;
@ -338,20 +346,18 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) {
bool AP_Landing::type_slope_is_flaring(void) const
{
return in_progress && type_slope_stage == SLOPE_FINAL;
return (type_slope_stage == SLOPE_STAGE_FINAL);
}
bool AP_Landing::type_slope_is_on_approach(void) const
{
return in_progress &&
(type_slope_stage == SLOPE_APPROACH ||
type_slope_stage == SLOPE_PREFLARE);
return (type_slope_stage == SLOPE_STAGE_APPROACH ||
type_slope_stage == SLOPE_STAGE_PREFLARE);
}
bool AP_Landing::type_slope_is_expecting_impact(void) const
{
return in_progress &&
(type_slope_stage == SLOPE_PREFLARE ||
type_slope_stage == SLOPE_FINAL);
return (type_slope_stage == SLOPE_STAGE_PREFLARE ||
type_slope_stage == SLOPE_STAGE_FINAL);
}