Plane: Move auto_state variables to AP_landing members

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();

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 {

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;

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;

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));

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 {

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;

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) {

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.

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;

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;