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

AP_Landing: assign values to each of the slope approach stages 

these are logged so should be defined
This commit is contained in:
Peter Barker 2022-11-29 17:29:35 +11:00 committed by Peter Barker
parent 40b318318d
commit 979f7a0cae
2 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Landing/AP_Landing.h
View File

@ -169,11 +169,11 @@ private:
// Land Type STANDARD GLIDE SLOPE // Land Type STANDARD GLIDE SLOPE
enum { enum class SlopeStage {
SLOPE_STAGE_NORMAL, NORMAL = 0,
SLOPE_STAGE_APPROACH, APPROACH = 1,
SLOPE_STAGE_PREFLARE, PREFLARE = 2,
SLOPE_STAGE_FINAL FINAL = 3,
} type_slope_stage; } type_slope_stage;
struct { struct {

40
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -33,7 +33,7 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons
// once landed, post some landing statistics to the GCS // once landed, post some landing statistics to the GCS
type_slope_flags.post_stats = false; type_slope_flags.post_stats = false;
type_slope_stage = SLOPE_STAGE_NORMAL; type_slope_stage = SlopeStage::NORMAL;
gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
} }
@ -57,7 +57,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
// adjust final landing parameters // adjust final landing parameters
// determine stage // determine stage
if (type_slope_stage == SLOPE_STAGE_NORMAL) { if (type_slope_stage == SlopeStage::NORMAL) {
const bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale(); 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 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; const bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
@ -65,7 +65,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
(wp_proportion >= 0 && heading_lined_up && on_flight_line) || (wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
(wp_proportion > 0.15f && heading_lined_up && below_prev_WP) || (wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
(wp_proportion > 0.5f)) { (wp_proportion > 0.5f)) {
type_slope_stage = SLOPE_STAGE_APPROACH; type_slope_stage = SlopeStage::APPROACH;
} }
} }
@ -99,7 +99,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
(!rangefinder_state_in_range && wp_proportion >= 1) || (!rangefinder_state_in_range && wp_proportion >= 1) ||
probably_crashed) { probably_crashed) {
if (type_slope_stage != SLOPE_STAGE_FINAL) { if (type_slope_stage != SlopeStage::FINAL) {
type_slope_flags.post_stats = true; type_slope_flags.post_stats = true;
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
@ -110,7 +110,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
(double)current_loc.get_distance(next_WP_loc)); (double)current_loc.get_distance(next_WP_loc));
} }
type_slope_stage = SLOPE_STAGE_FINAL; type_slope_stage = SlopeStage::FINAL;
// Check if the landing gear was deployed before landing // Check if the landing gear was deployed before landing
// If not - go around // If not - go around
@ -130,11 +130,11 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
aparm.min_gndspeed_cm.load(); aparm.min_gndspeed_cm.load();
aparm.throttle_cruise.load(); aparm.throttle_cruise.load();
} }
} else if (type_slope_stage == SLOPE_STAGE_APPROACH && pre_flare_airspeed > 0) { } else if (type_slope_stage == SlopeStage::APPROACH && pre_flare_airspeed > 0) {
bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt); 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); bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec);
if (reached_pre_flare_alt || reached_pre_flare_sec) { if (reached_pre_flare_alt || reached_pre_flare_sec) {
type_slope_stage = SLOPE_STAGE_PREFLARE; type_slope_stage = SlopeStage::PREFLARE;
} }
} }
@ -156,12 +156,12 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
} }
// check if we should auto-disarm after a confirmed landing // check if we should auto-disarm after a confirmed landing
if (type_slope_stage == SLOPE_STAGE_FINAL) { if (type_slope_stage == SlopeStage::FINAL) {
disarm_if_autoland_complete_fn(); disarm_if_autoland_complete_fn();
} }
if (mission.continue_after_land() && if (mission.continue_after_land() &&
type_slope_stage == SLOPE_STAGE_FINAL && type_slope_stage == SlopeStage::FINAL &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() < 1) { gps.ground_speed() < 1) {
/* /*
@ -342,14 +342,14 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void)
int32_t target_airspeed_cm = aparm.airspeed_cruise_cm; int32_t target_airspeed_cm = aparm.airspeed_cruise_cm;
switch (type_slope_stage) { switch (type_slope_stage) {
case SLOPE_STAGE_APPROACH: case SlopeStage::APPROACH:
if (land_airspeed >= 0) { if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100; target_airspeed_cm = land_airspeed * 100;
} }
break; break;
case SLOPE_STAGE_PREFLARE: case SlopeStage::PREFLARE:
case SLOPE_STAGE_FINAL: case SlopeStage::FINAL:
if (pre_flare_airspeed > 0) { if (pre_flare_airspeed > 0) {
// if we just preflared then continue using the pre-flare airspeed during final flare // if we just preflared then continue using the pre-flare airspeed during final flare
target_airspeed_cm = pre_flare_airspeed * 100; target_airspeed_cm = pre_flare_airspeed * 100;
@ -371,7 +371,7 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void)
int32_t AP_Landing::type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd) int32_t AP_Landing::type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
{ {
if (type_slope_stage == SLOPE_STAGE_FINAL) { if (type_slope_stage == SlopeStage::FINAL) {
return constrain_int32(desired_roll_cd, level_roll_limit_cd * -1, level_roll_limit_cd); return constrain_int32(desired_roll_cd, level_roll_limit_cd * -1, level_roll_limit_cd);
} else { } else {
return desired_roll_cd; return desired_roll_cd;
@ -380,24 +380,24 @@ int32_t AP_Landing::type_slope_constrain_roll(const int32_t desired_roll_cd, con
bool AP_Landing::type_slope_is_flaring(void) const bool AP_Landing::type_slope_is_flaring(void) const
{ {
return (type_slope_stage == SLOPE_STAGE_FINAL); return (type_slope_stage == SlopeStage::FINAL);
} }
bool AP_Landing::type_slope_is_on_approach(void) const bool AP_Landing::type_slope_is_on_approach(void) const
{ {
return (type_slope_stage == SLOPE_STAGE_APPROACH || return (type_slope_stage == SlopeStage::APPROACH ||
type_slope_stage == SLOPE_STAGE_PREFLARE); type_slope_stage == SlopeStage::PREFLARE);
} }
bool AP_Landing::type_slope_is_expecting_impact(void) const bool AP_Landing::type_slope_is_expecting_impact(void) const
{ {
return (type_slope_stage == SLOPE_STAGE_PREFLARE || return (type_slope_stage == SlopeStage::PREFLARE ||
type_slope_stage == SLOPE_STAGE_FINAL); type_slope_stage == SlopeStage::FINAL);
} }
bool AP_Landing::type_slope_is_complete(void) const bool AP_Landing::type_slope_is_complete(void) const
{ {
return (type_slope_stage == SLOPE_STAGE_FINAL); return (type_slope_stage == SlopeStage::FINAL);
} }
void AP_Landing::type_slope_log(void) const void AP_Landing::type_slope_log(void) const
@ -425,5 +425,5 @@ void AP_Landing::type_slope_log(void) const
bool AP_Landing::type_slope_is_throttle_suppressed(void) const bool AP_Landing::type_slope_is_throttle_suppressed(void) const
{ {
return type_slope_stage == SLOPE_STAGE_FINAL; return type_slope_stage == SlopeStage::FINAL;
} }