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
enum {
SLOPE_STAGE_NORMAL,
SLOPE_STAGE_APPROACH,
SLOPE_STAGE_PREFLARE,
SLOPE_STAGE_FINAL
enum class SlopeStage {
NORMAL = 0,
APPROACH = 1,
PREFLARE = 2,
FINAL = 3,
} type_slope_stage;
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
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);
}
@ -57,7 +57,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
// adjust final landing parameters
// 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 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;
@ -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.15f && heading_lined_up && below_prev_WP) ||
(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) ||
probably_crashed) {
if (type_slope_stage != SLOPE_STAGE_FINAL) {
if (type_slope_stage != SlopeStage::FINAL) {
type_slope_flags.post_stats = true;
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());
@ -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));
}
type_slope_stage = SLOPE_STAGE_FINAL;
type_slope_stage = SlopeStage::FINAL;
// Check if the landing gear was deployed before landing
// 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.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_sec = pre_flare_sec > 0 && (height <= sink_rate * 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
if (type_slope_stage == SLOPE_STAGE_FINAL) {
if (type_slope_stage == SlopeStage::FINAL) {
disarm_if_autoland_complete_fn();
}
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.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;
switch (type_slope_stage) {
case SLOPE_STAGE_APPROACH:
case SlopeStage::APPROACH:
if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
case SLOPE_STAGE_PREFLARE:
case SLOPE_STAGE_FINAL:
case SlopeStage::PREFLARE:
case SlopeStage::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;
@ -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)
{
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);
} else {
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
{
return (type_slope_stage == SLOPE_STAGE_FINAL);
return (type_slope_stage == SlopeStage::FINAL);
}
bool AP_Landing::type_slope_is_on_approach(void) const
{
return (type_slope_stage == SLOPE_STAGE_APPROACH ||
type_slope_stage == SLOPE_STAGE_PREFLARE);
return (type_slope_stage == SlopeStage::APPROACH ||
type_slope_stage == SlopeStage::PREFLARE);
}
bool AP_Landing::type_slope_is_expecting_impact(void) const
{
return (type_slope_stage == SLOPE_STAGE_PREFLARE ||
type_slope_stage == SLOPE_STAGE_FINAL);
return (type_slope_stage == SlopeStage::PREFLARE ||
type_slope_stage == SlopeStage::FINAL);
}
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
@ -425,5 +425,5 @@ void AP_Landing::type_slope_log(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;
}