mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Landing: re/move complete flag into backend
the complete flag was only ever true during FLIGHT_LAND_FINAL so we just check for that now instead
This commit is contained in:
parent
4cf1c74c62
commit
bdafc2c025
@ -286,7 +286,6 @@ void Plane::crash_detection_update(void)
|
|||||||
if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
|
if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
|
||||||
disarm_motors();
|
disarm_motors();
|
||||||
}
|
}
|
||||||
landing.complete = true;
|
|
||||||
if (crashed_near_land_waypoint) {
|
if (crashed_near_land_waypoint) {
|
||||||
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
|
||||||
} else {
|
} else {
|
||||||
|
@ -143,7 +143,6 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
|
|||||||
|
|
||||||
void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
|
void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
|
||||||
{
|
{
|
||||||
complete = false;
|
|
||||||
commanded_go_around = false;
|
commanded_go_around = false;
|
||||||
|
|
||||||
switch (type) {
|
switch (type) {
|
||||||
@ -183,7 +182,6 @@ bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &nex
|
|||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
|
type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -208,7 +206,6 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
|
|||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
|
type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -224,7 +221,6 @@ bool AP_Landing::is_flaring(void) const
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
return type_slope_is_flaring();
|
return type_slope_is_flaring();
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -240,7 +236,6 @@ bool AP_Landing::is_on_approach(void) const
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
return type_slope_is_on_approach();
|
return type_slope_is_on_approach();
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -256,7 +251,6 @@ bool AP_Landing::is_expecting_impact(void) const
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
return type_slope_is_expecting_impact();
|
return type_slope_is_expecting_impact();
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -278,7 +272,6 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo
|
|||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
|
type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -386,7 +379,6 @@ int32_t AP_Landing::get_target_airspeed_cm(void)
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
return type_slope_get_target_airspeed_cm();
|
return type_slope_get_target_airspeed_cm();
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return SpdHgt_Controller->get_land_airspeed();
|
return SpdHgt_Controller->get_land_airspeed();
|
||||||
}
|
}
|
||||||
@ -401,7 +393,6 @@ bool AP_Landing::request_go_around(void)
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case TYPE_STANDARD_GLIDE_SLOPE:
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
return type_slope_request_go_around();
|
return type_slope_request_go_around();
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -413,4 +404,17 @@ void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage)
|
|||||||
commanded_go_around = false;
|
commanded_go_around = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* returns true when a landing is complete, usually used to disable throttle
|
||||||
|
*/
|
||||||
|
bool AP_Landing::is_complete(void) const
|
||||||
|
{
|
||||||
|
switch (type) {
|
||||||
|
case TYPE_STANDARD_GLIDE_SLOPE:
|
||||||
|
return type_slope_is_complete();
|
||||||
|
default:
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -97,14 +97,10 @@ public:
|
|||||||
int8_t get_flap_percent(void) const { return flap_percent; }
|
int8_t get_flap_percent(void) const { return flap_percent; }
|
||||||
int8_t get_throttle_slewrate(void) const { return throttle_slewrate; }
|
int8_t get_throttle_slewrate(void) const { return throttle_slewrate; }
|
||||||
bool is_commanded_go_around(void) const { return commanded_go_around; }
|
bool is_commanded_go_around(void) const { return commanded_go_around; }
|
||||||
bool is_complete(void) const { return complete; }
|
bool is_complete(void) const;
|
||||||
void set_initial_slope(void) { initial_slope = slope; }
|
void set_initial_slope(void) { initial_slope = slope; }
|
||||||
bool is_expecting_impact(void) const;
|
bool is_expecting_impact(void) const;
|
||||||
|
|
||||||
// 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;
|
|
||||||
|
|
||||||
// landing altitude offset (meters)
|
// landing altitude offset (meters)
|
||||||
float alt_offset;
|
float alt_offset;
|
||||||
|
|
||||||
@ -176,6 +172,7 @@ private:
|
|||||||
void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
|
void type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
|
||||||
int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
|
int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
|
||||||
bool type_slope_request_go_around(void);
|
bool type_slope_request_go_around(void);
|
||||||
|
bool type_slope_is_complete(void) const;
|
||||||
bool type_slope_is_flaring(void) const;
|
bool type_slope_is_flaring(void) const;
|
||||||
bool type_slope_is_on_approach(void) const;
|
bool type_slope_is_on_approach(void) const;
|
||||||
bool type_slope_is_expecting_impact(void) const;
|
bool type_slope_is_expecting_impact(void) const;
|
||||||
|
@ -38,7 +38,6 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo
|
|||||||
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
|
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
|
||||||
// the altitude has been reached, restart the landing sequence
|
// the altitude has been reached, restart the landing sequence
|
||||||
throttle_suppressed = false;
|
throttle_suppressed = false;
|
||||||
complete = false;
|
|
||||||
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
|
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -94,7 +93,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 (!complete) {
|
if (type_slope_stage != SLOPE_STAGE_FINAL) {
|
||||||
post_stats = true;
|
post_stats = true;
|
||||||
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
|
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
|
||||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed());
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed());
|
||||||
@ -104,7 +103,6 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n
|
|||||||
(double)ahrs.get_gps().ground_speed(),
|
(double)ahrs.get_gps().ground_speed(),
|
||||||
(double)get_distance(current_loc, next_WP_loc));
|
(double)get_distance(current_loc, next_WP_loc));
|
||||||
}
|
}
|
||||||
complete = true;
|
|
||||||
type_slope_stage = SLOPE_STAGE_FINAL;
|
type_slope_stage = SLOPE_STAGE_FINAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -118,7 +116,7 @@ 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 (!complete && type_slope_stage == SLOPE_STAGE_APPROACH && pre_flare_airspeed > 0) {
|
} else if (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_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) {
|
||||||
@ -145,7 +143,7 @@ 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 (complete) {
|
if (type_slope_stage == SLOPE_STAGE_FINAL) {
|
||||||
disarm_if_autoland_complete_fn();
|
disarm_if_autoland_complete_fn();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -371,3 +369,9 @@ bool AP_Landing::type_slope_is_expecting_impact(void) const
|
|||||||
return (type_slope_stage == SLOPE_STAGE_PREFLARE ||
|
return (type_slope_stage == SLOPE_STAGE_PREFLARE ||
|
||||||
type_slope_stage == SLOPE_STAGE_FINAL);
|
type_slope_stage == SLOPE_STAGE_FINAL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool AP_Landing::type_slope_is_complete(void) const
|
||||||
|
{
|
||||||
|
return (type_slope_stage == SLOPE_STAGE_FINAL);
|
||||||
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user