diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index b227f0d8e8..785ba6eab4 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -355,7 +355,7 @@ void Plane::one_second_loop() update_home(); // reset the landing altitude correction - auto_state.land_alt_offset = 0; + landing.alt_offset = 0; } // update error mask of sensors and subsystems. The mask uses the @@ -544,14 +544,14 @@ void Plane::handle_auto_mode(void) calc_nav_roll(); calc_nav_pitch(); - if (auto_state.land_complete) { + if (landing.complete) { // during final approach constrain roll to the range // allowed for level flight nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); } calc_throttle(); - if (auto_state.land_complete) { + if (landing.complete) { // we are in the final stage of a landing - force // zero throttle channel_throttle->set_servo_out(0); @@ -562,8 +562,8 @@ void Plane::handle_auto_mode(void) if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } - auto_state.land_complete = false; - auto_state.land_pre_flare = false; + landing.complete = false; + landing.pre_flare = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); @@ -798,7 +798,7 @@ void Plane::update_navigation() set_mode(QRTL, MODE_REASON_UNKNOWN); break; } else if (g.rtl_autoland == 1 && - !auto_state.checked_for_autoland && + !landing.checked_for_autoland && reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence @@ -806,16 +806,16 @@ void Plane::update_navigation() // prevent running the expensive jump_to_landing_sequence // on every loop - auto_state.checked_for_autoland = true; + landing.checked_for_autoland = true; } else if (g.rtl_autoland == 2 && - !auto_state.checked_for_autoland) { + !landing.checked_for_autoland) { // Go directly to the landing sequence jump_to_landing_sequence(); // prevent running the expensive jump_to_landing_sequence // on every loop - auto_state.checked_for_autoland = true; + landing.checked_for_autoland = true; } radius = abs(g.rtl_radius); if (radius > 0) { @@ -864,7 +864,7 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs) switch (fs) { case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude()); - auto_state.land_in_progress = true; + landing.in_progress = true; #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable == 1) { if (! geofence_set_enabled(false, AUTO_TOGGLED)) { @@ -884,18 +884,18 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs) case AP_SpdHgtControl::FLIGHT_LAND_ABORT: gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100); - auto_state.land_in_progress = false; + landing.in_progress = false; break; case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - auto_state.land_in_progress = true; + landing.in_progress = true; break; case AP_SpdHgtControl::FLIGHT_NORMAL: case AP_SpdHgtControl::FLIGHT_VTOL: case AP_SpdHgtControl::FLIGHT_TAKEOFF: - auto_state.land_in_progress = false; + landing.in_progress = false; break; } @@ -935,14 +935,14 @@ void Plane::update_alt() if (auto_throttle_mode && !throttle_suppressed) { float distance_beyond_land_wp = 0; - if (auto_state.land_in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { + if (landing.in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = get_distance(current_loc, next_WP_loc); } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, - auto_state.land_in_progress, + landing.in_progress, distance_beyond_land_wp, get_takeoff_pitch_min_cd(), throttle_nudge, @@ -970,9 +970,9 @@ void Plane::update_flight_stage(void) flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT); - } else if (auto_state.land_complete == true) { + } else if (landing.complete == true) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); - } else if (auto_state.land_pre_flare == true) { + } else if (landing.pre_flare == true) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); } else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale(); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index e7f2ccc8b3..0c69a02b6b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -457,16 +457,6 @@ private: // Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process. bool takeoff_complete:1; - // 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 land_complete:1; - - // Flag to indicate if we have triggered pre-flare. This occurs when we have reached LAND_PF_ALT - bool land_pre_flare:1; - - // are we in auto and flight mode is approach || pre-flare || final (flare) - bool land_in_progress:1; - // are we headed to the land approach waypoint? Works for any nav type bool wp_is_land_approach:1; @@ -482,9 +472,6 @@ private: // in FBWA taildragger takeoff mode bool fbwa_tdrag_takeoff_mode:1; - // have we checked for an auto-land? - bool checked_for_autoland:1; - // denotes if a go-around has been commanded for landing bool commanded_go_around:1; @@ -532,18 +519,9 @@ private: // last time is_flying() returned true in milliseconds uint32_t last_flying_ms; - // once landed, post some landing statistics to the GCS - bool post_landing_stats; - // time stamp of when we start flying while in auto mode in milliseconds uint32_t started_flying_in_auto_ms; - // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - aparm.land_flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc) - float land_slope; - - // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope - float initial_land_slope; - // barometric altitude at start of takeoff float baro_takeoff_alt; @@ -552,9 +530,6 @@ private: // are we doing loiter mode as a VTOL? bool vtol_loiter:1; - - // landing altitude offset (meters) - float land_alt_offset; } auto_state; struct { diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 3abb3d0123..58e30a1db9 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -473,10 +473,10 @@ float Plane::mission_alt_offset(void) { float ret = g.alt_offset; if (control_mode == AUTO && - (auto_state.land_in_progress || auto_state.wp_is_land_approach)) { + (landing.in_progress || auto_state.wp_is_land_approach)) { // when landing after an aborted landing due to too high glide // slope we use an offset from the last landing attempt - ret += auto_state.land_alt_offset; + ret += landing.alt_offset; } return ret; } @@ -657,7 +657,7 @@ void Plane::rangefinder_height_update(void) if (now - rangefinder_state.last_correction_time_ms > 5000) { rangefinder_state.correction = correction; rangefinder_state.initial_correction = correction; - auto_state.initial_land_slope = auto_state.land_slope; + landing.initial_slope = landing.slope; rangefinder_state.last_correction_time_ms = now; } else { rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction; diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index 9399083df8..e0c8a56dd2 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -25,7 +25,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob // take no action in some flight modes if (plane.control_mode == MANUAL || (plane.control_mode == AUTO && !plane.auto_state.takeoff_complete) || - (plane.control_mode == AUTO && plane.auto_state.land_in_progress) || // TODO: consider allowing action during approach + (plane.control_mode == AUTO && plane.landing.in_progress) || // TODO: consider allowing action during approach plane.control_mode == AUTOTUNE || plane.control_mode == QLAND) { actual_action = MAV_COLLISION_ACTION_NONE; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 3c07be7536..6d53e8b2c1 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -16,8 +16,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // special handling for nav vs non-nav commands if (AP_Mission::is_nav_cmd(cmd)) { // set land_complete to false to stop us zeroing the throttle - auto_state.land_complete = false; - auto_state.land_pre_flare = false; + landing.complete = false; + landing.pre_flare = false; auto_state.sink_rate = 0; // set takeoff_complete to true so we don't add extra elevator @@ -31,7 +31,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) auto_state.idle_mode = false; // once landed, post some landing statistics to the GCS - auto_state.post_landing_stats = false; + landing.post_stats = false; nav_controller->set_data_is_stale(); @@ -399,7 +399,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_pitch_cd = 1000; } - auto_state.land_slope = 0; + landing.slope = 0; // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 847f03e43b..c01edc7115 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -295,7 +295,7 @@ void Plane::crash_detection_update(void) if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } - auto_state.land_complete = true; + landing.complete = true; if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 00862fce12..13852e6db8 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -19,8 +19,8 @@ bool Plane::verify_land() if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { throttle_suppressed = false; - auto_state.land_complete = false; - auto_state.land_pre_flare = false; + landing.complete = false; + landing.pre_flare = false; nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); // see if we have reached abort altitude @@ -71,8 +71,8 @@ bool Plane::verify_land() (!rangefinder_in_range && auto_state.wp_proportion >= 1) || probably_crashed) { - if (!auto_state.land_complete) { - auto_state.post_landing_stats = true; + if (!landing.complete) { + landing.post_stats = true; if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); } else { @@ -81,7 +81,7 @@ bool Plane::verify_land() (double)gps.ground_speed(), (double)get_distance(current_loc, next_WP_loc)); } - auto_state.land_complete = true; + landing.complete = true; update_flight_stage(); } @@ -95,11 +95,11 @@ bool Plane::verify_land() g.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } - } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) { + } else if (!landing.complete && !landing.pre_flare && aparm.land_pre_flare_airspeed > 0) { bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt); bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec); if (reached_pre_flare_alt || reached_pre_flare_sec) { - auto_state.land_pre_flare = true; + landing.pre_flare = true; update_flight_stage(); } } @@ -117,8 +117,8 @@ bool Plane::verify_land() // once landed and stationary, post some statistics // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm - if (auto_state.post_landing_stats && !arming.is_armed()) { - auto_state.post_landing_stats = false; + if (landing.post_stats && !arming.is_armed()) { + landing.post_stats = false; gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); } @@ -188,15 +188,15 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) // offset and "perfect" slope. // calculate projected slope with projected alt - float new_slope_deg = degrees(atan(auto_state.land_slope)); - float initial_slope_deg = degrees(atan(auto_state.initial_land_slope)); + float new_slope_deg = degrees(atan(landing.slope)); + float initial_slope_deg = degrees(atan(landing.initial_slope)); // is projected slope too steep? if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); - auto_state.land_alt_offset = rangefinder_state.correction; + landing.alt_offset = rangefinder_state.correction; auto_state.commanded_go_around = 1; g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once } @@ -251,10 +251,10 @@ void Plane::setup_landing_glide_slope(void) } // calculate slope to landing point - bool is_first_calc = is_zero(auto_state.land_slope); - auto_state.land_slope = (sink_height - aim_height) / total_distance; + bool is_first_calc = is_zero(landing.slope); + landing.slope = (sink_height - aim_height) / total_distance; if (is_first_calc) { - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope))); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(landing.slope))); } @@ -283,7 +283,7 @@ void Plane::setup_landing_glide_slope(void) // calculate point along that slope 500m ahead location_update(loc, land_bearing_cd*0.01f, land_projection); - loc.alt -= auto_state.land_slope * land_projection * 100; + loc.alt -= landing.slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion() target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index d1e1166d88..581e24524a 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -108,7 +108,7 @@ void Plane::calc_airspeed_errors() case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - if (auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) { + if (landing.pre_flare && aparm.land_pre_flare_airspeed > 0) { // if we just preflared then continue using the pre-flare airspeed during final flare target_airspeed_cm = aparm.land_pre_flare_airspeed * 100; } else if (land_airspeed >= 0) { diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index d4c6000d48..6d50311b25 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -774,7 +774,7 @@ void Plane::set_servos(void) if (g.land_then_servos_neutral > 0 && control_mode == AUTO && g.land_disarm_delay > 0 && - auto_state.land_complete && + landing.complete && !arming.is_armed()) { // after an auto land and auto disarm, set the servos to be neutral just // in case we're upside down or some crazy angle and straining the servos. diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 53a94a99e9..7fdbe417ac 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -335,13 +335,13 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) auto_state.next_wp_no_crosstrack = true; // reset landing check - auto_state.checked_for_autoland = false; + landing.checked_for_autoland = false; // reset go around command auto_state.commanded_go_around = false; // not in pre-flare - auto_state.land_pre_flare = false; + landing.pre_flare = false; // zero locked course steer_state.locked_course_err = 0; diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 29d1c32cd4..3e960b284a 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -32,6 +32,31 @@ public: static const struct AP_Param::GroupInfo var_info[]; + // 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; + + // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - aparm.land_flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc) + float slope; + + // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope + float initial_slope; + + // landing altitude offset (meters) + float alt_offset; + + // once landed, post some landing statistics to the GCS + bool post_stats; + + // have we checked for an auto-land? + bool checked_for_autoland; + private: AP_Mission &mission;