diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 9b3968c240..dcb3df1282 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -138,112 +138,38 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @User: Standard AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE), - // @Param: DS_V_FWD - // @DisplayName: Deepstall forward velocity - // @Description: The forward velocity of the aircraft while stalled - // @Range: 0 20 - // @Units: m/s - // @User: Advanced - AP_GROUPINFO("DS_V_FWD", 15, AP_Landing, type_deepstall_forward_speed, 1), - - // @Param: DS_SLOPE_A - // @DisplayName: Deepstall slope a - // @Description: The a component of distance = a*wind + b - // @User: Advanced - AP_GROUPINFO("DS_SLOPE_A", 16, AP_Landing, type_deepstall_slope_a, 1), - - // @Param: DS_SLOPE_B - // @DisplayName: Deepstall slope b - // @Description: The a component of distance = a*wind + b - // @User: Advanced - AP_GROUPINFO("DS_SLOPE_B", 17, AP_Landing, type_deepstall_slope_b, 1), - - // @Param: DS_APP_EXT - // @DisplayName: Deepstall approach extension - // @Description: The forward velocity of the aircraft while stalled - // @Range: 10 200 - // @Units: meters - // @User: Advanced - AP_GROUPINFO("DS_APP_EXT", 18, AP_Landing, type_deepstall_approach_extension, 50), - - // @Param: DS_V_DWN - // @DisplayName: Deepstall veloicty down - // @Description: The downward velocity of the aircraft while stalled - // @Range: 0 20 - // @Units: m/s - // @User: Advanced - AP_GROUPINFO("DS_V_DWN", 19, AP_Landing, type_deepstall_down_speed, 2), - - // @Param: DS_SLEW_SPD - // @DisplayName: Deepstall slew speed - // @Description: The speed at which the elevator slews to deepstall - // @Range: 0 2 - // @Units: seconds - // @User: Advanced - AP_GROUPINFO("DS_SLEW_SPD", 20, AP_Landing, type_deepstall_slew_speed, 0.5), - - // @Param: DS_ELEV_PWM - // @DisplayName: Deepstall elevator PWM - // @Description: The PWM value for the elevator at full deflection in deepstall - // @Range: 900 2100 - // @Units: PWM - // @User: Advanced - AP_GROUPINFO("DS_ELEV_PWM", 21, AP_Landing, type_deepstall_elevator_pwm, 1500), - - // @Param: DS_ARSP_MAX - // @DisplayName: Deepstall enabled airspeed - // @Description: The maximum aispeed where the deepstall steering controller is allowed to have control - // @Range: 5 20 - // @Units: m/s - // @User: Advanced - AP_GROUPINFO("DS_ARSP_MAX", 22, AP_Landing, type_deepstall_handoff_airspeed, 15.0), - - // @Param: DS_ARSP_MIN - // @DisplayName: Deepstall minimum derating airspeed - // @Description: Deepstall lowest airspeed where the deepstall controller isn't allowed full control - // @Range: 5 20 - // @Units: m/s - // @User: Advanced - AP_GROUPINFO("DS_ARSP_MIN", 23, AP_Landing, type_deepstall_handoff_lower_limit_airspeed, 10.0), - - // @Param: DS_L1 - // @DisplayName: Deepstall L1 period - // @Description: Deepstall L1 navigational controller period - // @Range: 5 50 - // @Units: meters - // @User: Advanced - AP_GROUPINFO("DS_L1", 24, AP_Landing, type_deepstall_L1_period, 30.0), - - // @Param: DS_L1_I - // @DisplayName: Deepstall L1 I gain - // @Description: Deepstall L1 integratior gain - // @Range: 0 1 - // @User: Advanced - AP_GROUPINFO("DS_L1_I", 25, AP_Landing, type_deepstall_L1_i, 0), - - // @Param: DS_YAW_LIM - // @DisplayName: Deepstall yaw rate limit - // @Description: The yaw rate limit while navigating in deepstall - // @Range: 0 90 - // @Units degrees per second - // @User: Advanced - AP_GROUPINFO("DS_YAW_LIM", 26, AP_Landing, type_deepstall_yaw_rate_limit, 10), - - // @Param: DS_L1_TCON - // @DisplayName: Deepstall L1 time constant - // @Description: Time constant for deepstall L1 control - // @Range: 0 1 - // @Units seconds - // @User: Advanced - AP_GROUPINFO("DS_L1_TCON", 27, AP_Landing, type_deepstall_time_constant, 0.4), - // @Group: DS_ - // @Path: ../PID/PID.cpp - AP_SUBGROUPINFO(type_deepstall_PID, "DS_", 28, AP_Landing, PID), - + // @Path: AP_Landing_Deepstall.cpp + AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall), + AP_GROUPEND }; + // constructor +AP_Landing::AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm, + set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn, + constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn, + adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn, + adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn, + disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn, + update_flight_stage_fn_t _update_flight_stage_fn) : + mission(_mission) + ,ahrs(_ahrs) + ,SpdHgt_Controller(_SpdHgt_Controller) + ,nav_controller(_nav_controller) + ,aparm(_aparm) + ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn) + ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn) + ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn) + ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn) + ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn) + ,update_flight_stage_fn(_update_flight_stage_fn) + ,deepstall(*this) +{ + AP_Param::setup_object_defaults(this, var_info); +} + + void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) { log(); // log old state so we get a nice transition from old to new here @@ -255,7 +181,7 @@ void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float rel type_slope_do_land(cmd, relative_altitude); break; case TYPE_DEEPSTALL: - type_deepstall_do_land(cmd, relative_altitude); + deepstall.do_land(cmd, relative_altitude); break; default: // a incorrect type is handled in the verify_land @@ -280,8 +206,8 @@ bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); break; case TYPE_DEEPSTALL: - success = type_deepstall_verify_land(prev_WP_loc, next_WP_loc, current_loc, - height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); + success = deepstall.verify_land(prev_WP_loc, next_WP_loc, current_loc, + height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); break; default: // returning TRUE while executing verify_land() will increment the @@ -303,7 +229,7 @@ bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &nex type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); break; case TYPE_DEEPSTALL: - type_deepstall_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); + deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); break; default: break; @@ -414,7 +340,7 @@ bool AP_Landing::override_servos(void) { switch (type) { case TYPE_DEEPSTALL: - return type_deepstall_override_servos(); + return deepstall.override_servos(); case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -427,7 +353,7 @@ const DataFlash_Class::PID_Info* AP_Landing::get_pid_info(void) const { switch (type) { case TYPE_DEEPSTALL: - return &type_deepstall_get_pid_info(); + return &deepstall.get_pid_info(); case TYPE_STANDARD_GLIDE_SLOPE: default: return nullptr; @@ -529,7 +455,7 @@ bool AP_Landing::get_target_altitude_location(Location &location) switch (type) { case TYPE_DEEPSTALL: - return type_deepstall_get_target_altitude_location(location); + return deepstall.get_target_altitude_location(location); case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -577,7 +503,7 @@ int32_t AP_Landing::get_target_airspeed_cm(void) case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_get_target_airspeed_cm(); case TYPE_DEEPSTALL: - return type_deepstall_get_target_airspeed_cm(); + return deepstall.get_target_airspeed_cm(); default: // don't return the landing airspeed, because if type is invalid we have // no postive indication that the land airspeed has been configured or @@ -599,7 +525,7 @@ bool AP_Landing::request_go_around(void) success = type_slope_request_go_around(); break; case TYPE_DEEPSTALL: - success = type_deepstall_request_go_around(); + success = deepstall.request_go_around(); break; default: break; @@ -657,7 +583,7 @@ bool AP_Landing::is_throttle_suppressed(void) const case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_throttle_suppressed(); case TYPE_DEEPSTALL: - return type_deepstall_is_throttle_suppressed(); + return deepstall.is_throttle_suppressed(); default: return false; } diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 6d79ae2225..b321488220 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -27,6 +27,7 @@ class AP_Landing { public: + friend class AP_Landing_Deepstall; FUNCTOR_TYPEDEF(set_target_altitude_proportion_fn_t, void, const Location&, float); FUNCTOR_TYPEDEF(constrain_target_altitude_location_fn_t, void, const Location&, const Location&); @@ -37,27 +38,12 @@ public: // constructor AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm, - set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn, - constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn, - adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn, - adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn, - disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn, - update_flight_stage_fn_t _update_flight_stage_fn): - mission(_mission) - ,ahrs(_ahrs) - ,SpdHgt_Controller(_SpdHgt_Controller) - ,nav_controller(_nav_controller) - ,aparm(_aparm) - ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn) - ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn) - ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn) - ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn) - ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn) - ,update_flight_stage_fn(_update_flight_stage_fn) - { - AP_Param::setup_object_defaults(this, var_info); - } - + set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn, + constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn, + adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn, + adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn, + disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn, + update_flight_stage_fn_t _update_flight_stage_fn); // NOTE: make sure to update is_type_valid() @@ -94,6 +80,7 @@ public: // accessor functions for the params and states static const struct AP_Param::GroupInfo var_info[]; + int16_t get_pitch_cd(void) const { return pitch_cd; } float get_flare_sec(void) const { return flare_sec; } int8_t get_disarm_delay(void) const { return disarm_delay; } @@ -132,6 +119,9 @@ private: AP_SpdHgtControl *SpdHgt_Controller; AP_Navigation *nav_controller; + // support for deepstall landings + AP_Landing_Deepstall deepstall; + AP_Vehicle::FixedWing &aparm; set_target_altitude_proportion_fn_t set_target_altitude_proportion_fn; @@ -155,21 +145,6 @@ private: AP_Int8 flap_percent; AP_Int8 throttle_slewrate; AP_Int8 type; - AP_Float type_deepstall_forward_speed; - AP_Float type_deepstall_slope_a; - AP_Float type_deepstall_slope_b; - AP_Float type_deepstall_approach_extension; - AP_Float type_deepstall_down_speed; - AP_Float type_deepstall_slew_speed; - AP_Int16 type_deepstall_elevator_pwm; - AP_Float type_deepstall_handoff_airspeed; - AP_Float type_deepstall_handoff_lower_limit_airspeed; - AP_Float type_deepstall_L1_period; - AP_Float type_deepstall_L1_i; - AP_Float type_deepstall_yaw_rate_limit; - AP_Float type_deepstall_time_constant; - - static const DataFlash_Class::PID_Info empty_pid; // Land Type STANDARD GLIDE SLOPE @@ -205,56 +180,4 @@ private: bool type_slope_is_on_approach(void) const; bool type_slope_is_expecting_impact(void) const; bool type_slope_is_throttle_suppressed(void) const; - - // Landing type TYPE_DEEPSTALL - - //public AP_Landing interface - void type_deepstall_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); - void type_deepstall_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed); - bool type_deepstall_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, - const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, - const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range); - void type_deepstall_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, - const Location ¤t_loc, int32_t &target_altitude_offset_cm); - bool type_deepstall_override_servos(void); - bool type_deepstall_request_go_around(void); - bool type_deepstall_get_target_altitude_location(Location &location); - int32_t type_deepstall_get_target_airspeed_cm(void) const; - bool type_deepstall_is_throttle_suppressed(void) const; - - const DataFlash_Class::PID_Info& type_deepstall_get_pid_info(void) const; - - //private helpers - void type_deepstall_build_approach_path(); - float type_deepstall_predict_travel_distance(const Vector3f wind, const float height) const; - bool type_deepstall_verify_breakout(const Location ¤t_loc, const Location &target_loc, const float height_error) const; - float type_deepstall_update_steering(void); - - // deepstall members - enum deepstall_stage { - DEEPSTALL_STAGE_FLY_TO_LANDING, // fly to the deepstall landing point - DEEPSTALL_STAGE_ESTIMATE_WIND, // loiter until we have a decent estimate of the wind for the target altitude - DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT, // wait until the aircraft is aligned for the optimal breakout - DEEPSTALL_STAGE_FLY_TO_ARC, // fly to the start of the arc - DEEPSTALL_STAGE_ARC, // fly the arc - DEEPSTALL_STAGE_APPROACH, // fly the approach in, and prepare to deepstall when close - DEEPSTALL_STAGE_LAND, // the aircraft will stall torwards the ground while targeting a given point - }; - deepstall_stage type_deepstall_stage; - Location type_deepstall_landing_point; - Location type_deepstall_extended_approach; - Location type_deepstall_breakout_location; - Location type_deepstall_arc; - Location type_deepstall_arc_entry; - Location type_deepstall_arc_exit; - float type_deepstall_target_heading_deg; // target heading for the deepstall in degrees - uint32_t type_deepstall_stall_entry_time; // time when the aircrafted enter the stall (in millis) - uint16_t type_deepstall_initial_elevator_pwm; // PWM to start slewing the elevator up from - uint32_t type_deepstall_last_time; // last time the controller ran - float type_deepstall_L1_xtrack_i; // L1 integrator for navigation - PID type_deepstall_PID; - int32_t type_deepstall_last_target_bearing; // used for tracking the progress on loitering - int32_t type_deepstall_loiter_sum_cd; // used for tracking the progress on loitering - - #define DEEPSTALL_LOITER_ALT_TOLERANCE 5.0f }; diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index a68b3a4c21..2785f3ac18 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -22,140 +22,250 @@ #include #include +// table of user settable parameters for deepstall +const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = { + + // @Param: DS_V_FWD + // @DisplayName: Deepstall forward velocity + // @Description: The forward velocity of the aircraft while stalled + // @Range: 0 20 + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("V_FWD", 1, AP_Landing_Deepstall, forward_speed, 1), + + // @Param: DS_SLOPE_A + // @DisplayName: Deepstall slope a + // @Description: The a component of distance = a*wind + b + // @User: Advanced + AP_GROUPINFO("SLOPE_A", 2, AP_Landing_Deepstall, slope_a, 1), + + // @Param: DS_SLOPE_B + // @DisplayName: Deepstall slope b + // @Description: The a component of distance = a*wind + b + // @User: Advanced + AP_GROUPINFO("SLOPE_B", 3, AP_Landing_Deepstall, slope_b, 1), + + // @Param: DS_APP_EXT + // @DisplayName: Deepstall approach extension + // @Description: The forward velocity of the aircraft while stalled + // @Range: 10 200 + // @Units: meters + // @User: Advanced + AP_GROUPINFO("APP_EXT", 4, AP_Landing_Deepstall, approach_extension, 50), + + // @Param: DS_V_DWN + // @DisplayName: Deepstall veloicty down + // @Description: The downward velocity of the aircraft while stalled + // @Range: 0 20 + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("V_DWN", 5, AP_Landing_Deepstall, down_speed, 2), + + // @Param: DS_SLEW_SPD + // @DisplayName: Deepstall slew speed + // @Description: The speed at which the elevator slews to deepstall + // @Range: 0 2 + // @Units: seconds + // @User: Advanced + AP_GROUPINFO("SLEW_SPD", 6, AP_Landing_Deepstall, slew_speed, 0.5), + + // @Param: DS_ELEV_PWM + // @DisplayName: Deepstall elevator PWM + // @Description: The PWM value for the elevator at full deflection in deepstall + // @Range: 900 2100 + // @Units: PWM + // @User: Advanced + AP_GROUPINFO("ELEV_PWM", 7, AP_Landing_Deepstall, elevator_pwm, 1500), + + // @Param: DS_ARSP_MAX + // @DisplayName: Deepstall enabled airspeed + // @Description: The maximum aispeed where the deepstall steering controller is allowed to have control + // @Range: 5 20 + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("ARSP_MAX", 8, AP_Landing_Deepstall, handoff_airspeed, 15.0), + + // @Param: DS_ARSP_MIN + // @DisplayName: Deepstall minimum derating airspeed + // @Description: Deepstall lowest airspeed where the deepstall controller isn't allowed full control + // @Range: 5 20 + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("ARSP_MIN", 9, AP_Landing_Deepstall, handoff_lower_limit_airspeed, 10.0), + + // @Param: DS_L1 + // @DisplayName: Deepstall L1 period + // @Description: Deepstall L1 navigational controller period + // @Range: 5 50 + // @Units: meters + // @User: Advanced + AP_GROUPINFO("L1", 10, AP_Landing_Deepstall, L1_period, 30.0), + + // @Param: DS_L1_I + // @DisplayName: Deepstall L1 I gain + // @Description: Deepstall L1 integratior gain + // @Range: 0 1 + // @User: Advanced + AP_GROUPINFO("L1_I", 11, AP_Landing_Deepstall, L1_i, 0), + + // @Param: DS_YAW_LIM + // @DisplayName: Deepstall yaw rate limit + // @Description: The yaw rate limit while navigating in deepstall + // @Range: 0 90 + // @Units degrees per second + // @User: Advanced + AP_GROUPINFO("YAW_LIM", 12, AP_Landing_Deepstall, yaw_rate_limit, 10), + + // @Param: DS_L1_TCON + // @DisplayName: Deepstall L1 time constant + // @Description: Time constant for deepstall L1 control + // @Range: 0 1 + // @Units seconds + // @User: Advanced + AP_GROUPINFO("L1_TCON", 13, AP_Landing_Deepstall, time_constant, 0.4), + + // @Group: DS_ + // @Path: ../PID/PID.cpp + AP_SUBGROUPINFO(ds_PID, "", 13, AP_Landing_Deepstall, PID), + + AP_GROUPEND +}; + + // if DEBUG_PRINTS is defined statustexts will be sent to the GCS for debug purposes //#define DEBUG_PRINTS -void AP_Landing::type_deepstall_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) +void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) { - type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_LANDING; - type_deepstall_PID.reset_I(); + stage = DEEPSTALL_STAGE_FLY_TO_LANDING; + ds_PID.reset_I(); // load the landing point in, the rest of path building is deferred for a better wind estimate - memcpy(&type_deepstall_landing_point, &cmd.content.location, sizeof(Location)); + memcpy(&landing_point, &cmd.content.location, sizeof(Location)); } // currently identical to the slope aborts -void AP_Landing::type_deepstall_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed) +void AP_Landing_Deepstall::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed) { // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence throttle_suppressed = false; - nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); + landing.nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); } /* update navigation for landing */ -bool AP_Landing::type_deepstall_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, +bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range) { - switch (type_deepstall_stage) { + switch (stage) { case DEEPSTALL_STAGE_FLY_TO_LANDING: - if (get_distance(current_loc, type_deepstall_landing_point) > 2 * aparm.loiter_radius) { - nav_controller->update_waypoint(current_loc, type_deepstall_landing_point); + if (get_distance(current_loc, landing_point) > 2 * landing.aparm.loiter_radius) { + landing.nav_controller->update_waypoint(current_loc, landing_point); return false; } - type_deepstall_stage = DEEPSTALL_STAGE_ESTIMATE_WIND; - type_deepstall_loiter_sum_cd = 0; // reset the loiter counter + stage = DEEPSTALL_STAGE_ESTIMATE_WIND; + loiter_sum_cd = 0; // reset the loiter counter // no break case DEEPSTALL_STAGE_ESTIMATE_WIND: { - nav_controller->update_loiter(type_deepstall_landing_point, aparm.loiter_radius, 1); - if (!nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { + landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1); + if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { // wait until the altitude is correct before considering a breakout return false; } // only count loiter progress when within the target altitude - int32_t target_bearing = nav_controller->target_bearing_cd(); - int32_t delta = wrap_180_cd(target_bearing - type_deepstall_last_target_bearing); + int32_t target_bearing = landing.nav_controller->target_bearing_cd(); + int32_t delta = wrap_180_cd(target_bearing - last_target_bearing); if (delta > 0) { // only accumulate turns in the correct direction - type_deepstall_loiter_sum_cd += delta; + loiter_sum_cd += delta; } - type_deepstall_last_target_bearing = target_bearing; - if (type_deepstall_loiter_sum_cd < 36000) { + last_target_bearing = target_bearing; + if (loiter_sum_cd < 36000) { // wait until we've done at least one complete loiter at the correct altitude - nav_controller->update_loiter(type_deepstall_landing_point, aparm.loiter_radius, 1); + landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1); return false; } - type_deepstall_stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT; + stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT; //compute optimal path for landing - type_deepstall_build_approach_path(); + build_approach_path(); // no break } case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT: - if (!type_deepstall_verify_breakout(current_loc, type_deepstall_arc_entry, height)) { - nav_controller->update_loiter(type_deepstall_landing_point, aparm.loiter_radius, 1); + if (!verify_breakout(current_loc, arc_entry, height)) { + landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1); return false; } - type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_ARC; - memcpy(&type_deepstall_breakout_location, ¤t_loc, sizeof(Location)); + stage = DEEPSTALL_STAGE_FLY_TO_ARC; + memcpy(&breakout_location, ¤t_loc, sizeof(Location)); // no break case DEEPSTALL_STAGE_FLY_TO_ARC: - if (get_distance(current_loc, type_deepstall_arc_entry) > 2 * aparm.loiter_radius) { - nav_controller->update_waypoint(type_deepstall_breakout_location, type_deepstall_arc_entry); + if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) { + landing.nav_controller->update_waypoint(breakout_location, arc_entry); return false; } - type_deepstall_stage = DEEPSTALL_STAGE_ARC; + stage = DEEPSTALL_STAGE_ARC; // no break case DEEPSTALL_STAGE_ARC: { - Vector2f groundspeed = ahrs.groundspeed_vector(); - if (!nav_controller->reached_loiter_target() || - (fabsf(wrap_180(type_deepstall_target_heading_deg - + Vector2f groundspeed = landing.ahrs.groundspeed_vector(); + if (!landing.nav_controller->reached_loiter_target() || + (fabsf(wrap_180(target_heading_deg - degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) { - nav_controller->update_loiter(type_deepstall_arc, aparm.loiter_radius, 1); + landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, 1); return false; } - type_deepstall_stage = DEEPSTALL_STAGE_APPROACH; + stage = DEEPSTALL_STAGE_APPROACH; } // no break case DEEPSTALL_STAGE_APPROACH: { Location entry_point; - nav_controller->update_waypoint(type_deepstall_arc_exit, type_deepstall_extended_approach); + landing.nav_controller->update_waypoint(arc_exit, extended_approach); float relative_alt_D; - ahrs.get_relative_position_D_home(relative_alt_D); + landing.ahrs.get_relative_position_D_home(relative_alt_D); - const float travel_distance = type_deepstall_predict_travel_distance(ahrs.wind_estimate(), -relative_alt_D); + const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D); - memcpy(&entry_point, &type_deepstall_landing_point, sizeof(Location)); - location_update(entry_point, type_deepstall_target_heading_deg + 180.0, travel_distance); + memcpy(&entry_point, &landing_point, sizeof(Location)); + location_update(entry_point, target_heading_deg + 180.0, travel_distance); - if (!location_passed_point(current_loc, type_deepstall_arc_exit, entry_point)) { - if (location_passed_point(current_loc, type_deepstall_arc_exit, type_deepstall_extended_approach)) { + if (!location_passed_point(current_loc, arc_exit, entry_point)) { + if (location_passed_point(current_loc, arc_exit, extended_approach)) { // this should never happen, but prevent against an indefinite fly away - type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_LANDING; + stage = DEEPSTALL_STAGE_FLY_TO_LANDING; } return false; } - type_deepstall_stage = DEEPSTALL_STAGE_LAND; - type_deepstall_stall_entry_time = AP_HAL::millis(); + stage = DEEPSTALL_STAGE_LAND; + stall_entry_time = AP_HAL::millis(); const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator); if (elevator != nullptr) { // take the last used elevator angle as the starting deflection // don't worry about bailing here if the elevator channel can't be found // that will be handled within override_servos - type_deepstall_initial_elevator_pwm = elevator->get_output_pwm(); + initial_elevator_pwm = elevator->get_output_pwm(); } - type_deepstall_L1_xtrack_i = 0; // reset the integrators + L1_xtrack_i = 0; // reset the integrators } // no break case DEEPSTALL_STAGE_LAND: // while in deepstall the only thing verify needs to keep the extended approach point sufficently far away - nav_controller->update_waypoint(current_loc, type_deepstall_extended_approach); + landing.nav_controller->update_waypoint(current_loc, extended_approach); return false; default: return true; } } -bool AP_Landing::type_deepstall_override_servos(void) +bool AP_Landing_Deepstall::override_servos(void) { - if (!(type_deepstall_stage == DEEPSTALL_STAGE_LAND)) { + if (!(stage == DEEPSTALL_STAGE_LAND)) { return false; } @@ -165,33 +275,33 @@ bool AP_Landing::type_deepstall_override_servos(void) // deepstalls are impossible without these channels, abort the process GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels"); - type_deepstall_request_go_around(); + request_go_around(); return false; } // calculate the progress on slewing the elevator float slew_progress = 1.0f; - if (type_deepstall_slew_speed > 0) { - slew_progress = (AP_HAL::millis() - type_deepstall_stall_entry_time) / (100.0f * type_deepstall_slew_speed); + if (slew_speed > 0) { + slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed); slew_progress = constrain_float (slew_progress, 0.0f, 1.0f); } // mix the elevator to the correct value - elevator->set_output_pwm(linear_interpolate(type_deepstall_initial_elevator_pwm, type_deepstall_elevator_pwm, + elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm, slew_progress, 0.0f, 1.0f)); // use the current airspeed to dictate the travel limits float airspeed; - ahrs.airspeed_estimate(&airspeed); + landing.ahrs.airspeed_estimate(&airspeed); // only allow the deepstall steering controller to run below the handoff airspeed - if (slew_progress >= 1.0f || airspeed <= type_deepstall_handoff_airspeed) { + if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) { // run the steering conntroller - float pid = type_deepstall_update_steering(); + float pid = update_steering(); - float travel_limit = constrain_float((type_deepstall_handoff_airspeed - airspeed) / - (type_deepstall_handoff_airspeed - type_deepstall_handoff_lower_limit_airspeed) * + float travel_limit = constrain_float((handoff_airspeed - airspeed) / + (handoff_airspeed - handoff_lower_limit_airspeed) * 0.5f + 0.5f, 0.5f, 1.0f); @@ -210,90 +320,90 @@ bool AP_Landing::type_deepstall_override_servos(void) return true; } -bool AP_Landing::type_deepstall_request_go_around(void) +bool AP_Landing_Deepstall::request_go_around(void) { - flags.commanded_go_around = true; + landing.flags.commanded_go_around = true; return true; } -bool AP_Landing::type_deepstall_is_throttle_suppressed(void) const +bool AP_Landing_Deepstall::is_throttle_suppressed(void) const { - return type_deepstall_stage == DEEPSTALL_STAGE_LAND; + return stage == DEEPSTALL_STAGE_LAND; } -bool AP_Landing::type_deepstall_get_target_altitude_location(Location &location) +bool AP_Landing_Deepstall::get_target_altitude_location(Location &location) { - memcpy(&location, &type_deepstall_landing_point, sizeof(Location)); + memcpy(&location, &landing_point, sizeof(Location)); return true; } -int32_t AP_Landing::type_deepstall_get_target_airspeed_cm(void) const +int32_t AP_Landing_Deepstall::get_target_airspeed_cm(void) const { - if (type_deepstall_stage == DEEPSTALL_STAGE_APPROACH || - type_deepstall_stage == DEEPSTALL_STAGE_LAND) { - return pre_flare_airspeed * 100; + if (stage == DEEPSTALL_STAGE_APPROACH || + stage == DEEPSTALL_STAGE_LAND) { + return landing.pre_flare_airspeed * 100; } else { - return aparm.airspeed_cruise_cm; + return landing.aparm.airspeed_cruise_cm; } } -const DataFlash_Class::PID_Info& AP_Landing::type_deepstall_get_pid_info(void) const +const DataFlash_Class::PID_Info& AP_Landing_Deepstall::get_pid_info(void) const { - return type_deepstall_PID.get_pid_info(); + return ds_PID.get_pid_info(); } -void AP_Landing::type_deepstall_build_approach_path(void) +void AP_Landing_Deepstall::build_approach_path(void) { - Vector3f wind = ahrs.wind_estimate(); + Vector3f wind = landing.ahrs.wind_estimate(); // TODO: Support a user defined approach heading - type_deepstall_target_heading_deg = (degrees(atan2f(-wind.y, -wind.x))); + target_heading_deg = (degrees(atan2f(-wind.y, -wind.x))); - memcpy(&type_deepstall_extended_approach, &type_deepstall_landing_point, sizeof(Location)); - memcpy(&type_deepstall_arc_exit, &type_deepstall_landing_point, sizeof(Location)); + memcpy(&extended_approach, &landing_point, sizeof(Location)); + memcpy(&arc_exit, &landing_point, sizeof(Location)); //extend the approach point to 1km away so that there is always a navigational target - location_update(type_deepstall_extended_approach, type_deepstall_target_heading_deg, 1000.0); + location_update(extended_approach, target_heading_deg, 1000.0); - float expected_travel_distance = type_deepstall_predict_travel_distance(wind, type_deepstall_landing_point.alt / 100); - float approach_extension = expected_travel_distance + type_deepstall_approach_extension; + float expected_travel_distance = predict_travel_distance(wind, landing_point.alt / 100); + float approach_extension_m = expected_travel_distance + approach_extension; // an approach extension of 0 can result in a divide by 0 - if (fabsf(approach_extension) < 1.0f) { - approach_extension = 1.0f; + if (fabsf(approach_extension_m) < 1.0f) { + approach_extension_m = 1.0f; } - location_update(type_deepstall_arc_exit, type_deepstall_target_heading_deg + 180, approach_extension); - memcpy(&type_deepstall_arc, &type_deepstall_arc_exit, sizeof(Location)); - memcpy(&type_deepstall_arc_entry, &type_deepstall_arc_exit, sizeof(Location)); + location_update(arc_exit, target_heading_deg + 180, approach_extension_m); + memcpy(&arc, &arc_exit, sizeof(Location)); + memcpy(&arc_entry, &arc_exit, sizeof(Location)); // TODO: Support loitering on either side of the approach path - location_update(type_deepstall_arc, type_deepstall_target_heading_deg + 90.0, aparm.loiter_radius); - location_update(type_deepstall_arc_entry, type_deepstall_target_heading_deg + 90.0, aparm.loiter_radius * 2); + location_update(arc, target_heading_deg + 90.0, landing.aparm.loiter_radius); + location_update(arc_entry, target_heading_deg + 90.0, landing.aparm.loiter_radius * 2); #ifdef DEBUG_PRINTS // TODO: Send this information via a MAVLink packet GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f", - (double)(type_deepstall_arc.lat / 1e7),(double)( type_deepstall_arc.lng / 1e7)); + (double)(arc.lat / 1e7),(double)( arc.lng / 1e7)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f", - (double)(type_deepstall_arc_entry.lat / 1e7), (double)(type_deepstall_arc_entry.lng / 1e7)); + (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f", - (double)(type_deepstall_arc_exit.lat / 1e7), (double)(type_deepstall_arc_exit.lng / 1e7)); + (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f", - (double)(type_deepstall_extended_approach.lat / 1e7), (double)(type_deepstall_extended_approach.lng / 1e7)); - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension, + (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7)); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m, (double)expected_travel_distance); - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)type_deepstall_target_heading_deg); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg); #endif // DEBUG_PRINTS } -float AP_Landing::type_deepstall_predict_travel_distance(const Vector3f wind, const float height) const +float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height) const { bool reverse = false; - float course = radians(type_deepstall_target_heading_deg); + float course = radians(target_heading_deg); // a forward speed of 0 will result in a divide by 0 - float forward_speed = MAX(type_deepstall_forward_speed, 0.1f); + float forward_speed_ms = MAX(forward_speed, 0.1f); Vector2f wind_vec(wind.x, wind.y); // work with the 2D component of wind float wind_length = MAX(wind_vec.length(), 0.05f); // always assume a slight wind to avoid divide by 0 @@ -302,7 +412,7 @@ float AP_Landing::type_deepstall_predict_travel_distance(const Vector3f wind, co float offset = course + atan2f(-wind.y, -wind.x) + M_PI; // estimator for how far the aircraft will travel while entering the stall - float stall_distance = type_deepstall_slope_a * wind_length * cosf(offset) + type_deepstall_slope_b; + float stall_distance = slope_a * wind_length * cosf(offset) + slope_b; float theta = acosf(constrain_float((wind_vec * course_vec) / wind_length, -1.0f, 1.0f)); if ((course_vec % wind_vec) > 0) { @@ -311,25 +421,25 @@ float AP_Landing::type_deepstall_predict_travel_distance(const Vector3f wind, co } float cross_component = sinf(theta) * wind_length; - float estimated_crab_angle = asinf(constrain_float(cross_component / forward_speed, -1.0f, 1.0f)); + float estimated_crab_angle = asinf(constrain_float(cross_component / forward_speed_ms, -1.0f, 1.0f)); if (reverse) { estimated_crab_angle *= -1; } - float estimated_forward = cosf(estimated_crab_angle) * forward_speed + cosf(theta) * wind_length; + float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length; #ifdef DEBUG_PRINTS - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance, estimated_forward * height / type_deepstall_down_speed + stall_distance); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance, estimated_forward * height / down_speed + stall_distance); #endif // DEBUG_PRINTS - return estimated_forward * height / type_deepstall_down_speed + stall_distance; + return estimated_forward * height / down_speed + stall_distance; } -bool AP_Landing::type_deepstall_verify_breakout(const Location ¤t_loc, const Location &target_loc, +bool AP_Landing_Deepstall::verify_breakout(const Location ¤t_loc, const Location &target_loc, const float height_error) const { Vector2f location_delta = location_diff(current_loc, target_loc); - const float heading_error = degrees(ahrs.groundspeed_vector().angle(location_delta)); + const float heading_error = degrees(landing.ahrs.groundspeed_vector().angle(location_delta)); // Check to see if the the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg) // of margin so that the altitude to be within 5 meters of desired @@ -341,49 +451,49 @@ bool AP_Landing::type_deepstall_verify_breakout(const Location ¤t_loc, con return false; } -float AP_Landing::type_deepstall_update_steering() +float AP_Landing_Deepstall::update_steering() { Location current_loc; - if (!ahrs.get_position(current_loc)) { + if (!landing.ahrs.get_position(current_loc)) { // panic if no position source is available - // continue the deepstall. but target just holding the wings held level as deepstall should be a minimal energy + // continue the but target just holding the wings held level as deepstall should be a minimal energy // configuration on the aircraft, and if a position isn't available aborting would be worse GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: No position available. Attempting to hold level"); - memcpy(¤t_loc, &type_deepstall_landing_point, sizeof(Location)); + memcpy(¤t_loc, &landing_point, sizeof(Location)); } uint32_t time = AP_HAL::millis(); - float dt = constrain_float(time - type_deepstall_last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0; - type_deepstall_last_time = time; + float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0; + last_time = time; - Vector2f ab = location_diff(type_deepstall_arc_exit, type_deepstall_extended_approach); + Vector2f ab = location_diff(arc_exit, extended_approach); ab.normalize(); - Vector2f a_air = location_diff(type_deepstall_arc_exit, current_loc); + Vector2f a_air = location_diff(arc_exit, current_loc); float crosstrack_error = a_air % ab; - float sine_nu1 = constrain_float(crosstrack_error / MAX(type_deepstall_L1_period, 0.1f), -0.7071f, 0.7107f); + float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f); float nu1 = asinf(sine_nu1); - if (type_deepstall_L1_i > 0) { - type_deepstall_L1_xtrack_i += nu1 * type_deepstall_L1_i / dt; - type_deepstall_L1_xtrack_i = constrain_float(type_deepstall_L1_xtrack_i, -0.5f, 0.5f); - nu1 += type_deepstall_L1_xtrack_i; + if (L1_i > 0) { + L1_xtrack_i += nu1 * L1_i / dt; + L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f); + nu1 += L1_xtrack_i; } - float desired_change = wrap_PI(radians(type_deepstall_target_heading_deg) + nu1 - ahrs.yaw); + float desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw); - float yaw_rate = ahrs.get_gyro().z; - float yaw_rate_limit = radians(type_deepstall_yaw_rate_limit); - float error = wrap_PI(constrain_float(desired_change / type_deepstall_time_constant, - -yaw_rate_limit, yaw_rate_limit) - yaw_rate); + float yaw_rate = landing.ahrs.get_gyro().z; + float yaw_rate_limit_rps = radians(yaw_rate_limit); + float error = wrap_PI(constrain_float(desired_change / time_constant, + -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate); #ifdef DEBUG_PRINTS GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f", (double)crosstrack_error, (double)error, (double)degrees(yaw_rate), - (double)location_diff(current_loc, type_deepstall_landing_point).length()); + (double)location_diff(current_loc, landing_point).length()); #endif // DEBUG_PRINTS - return type_deepstall_PID.get_pid(error); + return ds_PID.get_pid(error); } diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.h b/libraries/AP_Landing/AP_Landing_Deepstall.h new file mode 100644 index 0000000000..433642e62f --- /dev/null +++ b/libraries/AP_Landing/AP_Landing_Deepstall.h @@ -0,0 +1,108 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +class AP_Landing; + +/// @class AP_Landing +/// @brief Class managing ArduPlane landing methods +class AP_Landing_Deepstall +{ +private: + friend class AP_Landing; + + // constructor + AP_Landing_Deepstall(AP_Landing &_landing) : + landing(_landing) + { + AP_Param::setup_object_defaults(this, var_info); + } + + AP_Landing &landing; + + static const struct AP_Param::GroupInfo var_info[]; + + // deepstall members + enum deepstall_stage { + DEEPSTALL_STAGE_FLY_TO_LANDING, // fly to the deepstall landing point + DEEPSTALL_STAGE_ESTIMATE_WIND, // loiter until we have a decent estimate of the wind for the target altitude + DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT, // wait until the aircraft is aligned for the optimal breakout + DEEPSTALL_STAGE_FLY_TO_ARC, // fly to the start of the arc + DEEPSTALL_STAGE_ARC, // fly the arc + DEEPSTALL_STAGE_APPROACH, // fly the approach in, and prepare to deepstall when close + DEEPSTALL_STAGE_LAND, // the aircraft will stall torwards the ground while targeting a given point + }; + + AP_Float forward_speed; + AP_Float slope_a; + AP_Float slope_b; + AP_Float approach_extension; + AP_Float down_speed; + AP_Float slew_speed; + AP_Int16 elevator_pwm; + AP_Float handoff_airspeed; + AP_Float handoff_lower_limit_airspeed; + AP_Float L1_period; + AP_Float L1_i; + AP_Float yaw_rate_limit; + AP_Float time_constant; + int32_t loiter_sum_cd; // used for tracking the progress on loitering + deepstall_stage stage; + Location landing_point; + Location extended_approach; + Location breakout_location; + Location arc; + Location arc_entry; + Location arc_exit; + float target_heading_deg; // target heading for the deepstall in degrees + uint32_t stall_entry_time; // time when the aircrafted enter the stall (in millis) + uint16_t initial_elevator_pwm; // PWM to start slewing the elevator up from + uint32_t last_time; // last time the controller ran + float L1_xtrack_i; // L1 integrator for navigation + PID ds_PID; + int32_t last_target_bearing; // used for tracking the progress on loitering + + //public AP_Landing interface + void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); + void verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed); + bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, + const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, + const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range); + void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, + const Location ¤t_loc, int32_t &target_altitude_offset_cm); + bool override_servos(void); + bool request_go_around(void); + bool get_target_altitude_location(Location &location); + int32_t get_target_airspeed_cm(void) const; + bool is_throttle_suppressed(void) const; + + const DataFlash_Class::PID_Info& get_pid_info(void) const; + + //private helpers + void build_approach_path(); + float predict_travel_distance(const Vector3f wind, const float height) const; + bool verify_breakout(const Location ¤t_loc, const Location &target_loc, const float height_error) const; + float update_steering(void); + + #define DEEPSTALL_LOITER_ALT_TOLERANCE 5.0f +};