diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 9ba34611dc..9b3968c240 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -138,6 +138,109 @@ 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), + AP_GROUPEND }; @@ -151,6 +254,9 @@ void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float rel case TYPE_STANDARD_GLIDE_SLOPE: type_slope_do_land(cmd, relative_altitude); break; + case TYPE_DEEPSTALL: + type_deepstall_do_land(cmd, relative_altitude); + break; default: // a incorrect type is handled in the verify_land break; @@ -173,6 +279,10 @@ bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, success = type_slope_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; + 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); + break; default: // returning TRUE while executing verify_land() will increment the // mission index which in many cases will trigger an RTL for end-of-mission @@ -192,6 +302,9 @@ bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &nex case TYPE_STANDARD_GLIDE_SLOPE: 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); + break; default: break; } @@ -218,12 +331,12 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing 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); break; + case TYPE_DEEPSTALL: default: break; } } -// return true while the aircraft should be in a flaring state bool AP_Landing::is_flaring(void) const { if (!flags.in_progress) { @@ -233,6 +346,7 @@ bool AP_Landing::is_flaring(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_flaring(); + case TYPE_DEEPSTALL: default: return false; } @@ -252,6 +366,7 @@ bool AP_Landing::is_on_approach(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); + case TYPE_DEEPSTALL: default: return false; } @@ -267,6 +382,8 @@ bool AP_Landing::is_ground_steering_allowed(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); + case TYPE_DEEPSTALL: + return false; default: return true; } @@ -283,6 +400,7 @@ bool AP_Landing::is_expecting_impact(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_expecting_impact(); + case TYPE_DEEPSTALL: default: return false; } @@ -295,6 +413,8 @@ bool AP_Landing::override_servos(void) { } switch (type) { + case TYPE_DEEPSTALL: + return type_deepstall_override_servos(); case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -306,6 +426,8 @@ bool AP_Landing::override_servos(void) { const DataFlash_Class::PID_Info* AP_Landing::get_pid_info(void) const { switch (type) { + case TYPE_DEEPSTALL: + return &type_deepstall_get_pid_info(); case TYPE_STANDARD_GLIDE_SLOPE: default: return nullptr; @@ -327,6 +449,7 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo case TYPE_STANDARD_GLIDE_SLOPE: type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); break; + case TYPE_DEEPSTALL: default: break; } @@ -391,6 +514,7 @@ int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd); + case TYPE_DEEPSTALL: default: return desired_roll_cd; } @@ -404,6 +528,8 @@ bool AP_Landing::get_target_altitude_location(Location &location) } switch (type) { + case TYPE_DEEPSTALL: + return type_deepstall_get_target_altitude_location(location); case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -450,6 +576,8 @@ int32_t AP_Landing::get_target_airspeed_cm(void) switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_get_target_airspeed_cm(); + case TYPE_DEEPSTALL: + return type_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 @@ -470,6 +598,9 @@ bool AP_Landing::request_go_around(void) case TYPE_STANDARD_GLIDE_SLOPE: success = type_slope_request_go_around(); break; + case TYPE_DEEPSTALL: + success = type_deepstall_request_go_around(); + break; default: break; } @@ -494,6 +625,8 @@ bool AP_Landing::is_complete(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_complete(); + case TYPE_DEEPSTALL: + return false; default: return true; } @@ -505,6 +638,7 @@ void AP_Landing::log(void) const case TYPE_STANDARD_GLIDE_SLOPE: type_slope_log(); break; + case TYPE_DEEPSTALL: default: break; } @@ -522,6 +656,8 @@ bool AP_Landing::is_throttle_suppressed(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_throttle_suppressed(); + case TYPE_DEEPSTALL: + return type_deepstall_is_throttle_suppressed(); default: return false; } diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 87c08faa18..6d79ae2225 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -20,6 +20,7 @@ #include #include #include +#include "AP_Landing_Deepstall.h" /// @class AP_Landing /// @brief Class managing ArduPlane landing methods @@ -62,7 +63,7 @@ public: // NOTE: make sure to update is_type_valid() enum Landing_Type { TYPE_STANDARD_GLIDE_SLOPE = 0, -// TODO: TYPE_DEEPSTALL, + TYPE_DEEPSTALL = 1, // TODO: TYPE_PARACHUTE, // TODO: TYPE_HELICAL, }; @@ -154,6 +155,21 @@ 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 @@ -189,4 +205,56 @@ 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 new file mode 100644 index 0000000000..a68b3a4c21 --- /dev/null +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -0,0 +1,389 @@ +/* + 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 . + */ + +/* + * AP_Landing_Deepstall.cpp - Landing logic handler for ArduPlane for deepstall landings + */ + +#include "AP_Landing.h" +#include +#include +#include + +// 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) +{ + type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_LANDING; + type_deepstall_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)); +} + +// 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) +{ + // 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)); +} + + +/* + update navigation for landing + */ +bool AP_Landing::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) +{ + switch (type_deepstall_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); + return false; + } + type_deepstall_stage = DEEPSTALL_STAGE_ESTIMATE_WIND; + type_deepstall_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)) { + // 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); + if (delta > 0) { // only accumulate turns in the correct direction + type_deepstall_loiter_sum_cd += delta; + } + type_deepstall_last_target_bearing = target_bearing; + if (type_deepstall_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); + return false; + } + type_deepstall_stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT; + //compute optimal path for landing + type_deepstall_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); + return false; + } + type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_ARC; + memcpy(&type_deepstall_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); + return false; + } + type_deepstall_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 - + degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) { + nav_controller->update_loiter(type_deepstall_arc, aparm.loiter_radius, 1); + return false; + } + type_deepstall_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); + + float relative_alt_D; + ahrs.get_relative_position_D_home(relative_alt_D); + + const float travel_distance = type_deepstall_predict_travel_distance(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); + + 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)) { + // this should never happen, but prevent against an indefinite fly away + type_deepstall_stage = DEEPSTALL_STAGE_FLY_TO_LANDING; + } + return false; + } + type_deepstall_stage = DEEPSTALL_STAGE_LAND; + type_deepstall_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(); + } + type_deepstall_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); + return false; + default: + return true; + } +} + +bool AP_Landing::type_deepstall_override_servos(void) +{ + if (!(type_deepstall_stage == DEEPSTALL_STAGE_LAND)) { + return false; + } + + SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator); + + if (elevator == nullptr) { + // 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(); + 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); + 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, + slew_progress, 0.0f, 1.0f)); + + // use the current airspeed to dictate the travel limits + float airspeed; + 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) { + // run the steering conntroller + float pid = type_deepstall_update_steering(); + + + float travel_limit = constrain_float((type_deepstall_handoff_airspeed - airspeed) / + (type_deepstall_handoff_airspeed - type_deepstall_handoff_lower_limit_airspeed) * + 0.5f + 0.5f, + 0.5f, 1.0f); + + float output = constrain_float(pid, -travel_limit, travel_limit); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, output*4500); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500); + + } else { + // allow the normal servo control of the channel + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, + SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)); + } + + // hand off rudder control to deepstall controlled + return true; +} + +bool AP_Landing::type_deepstall_request_go_around(void) +{ + flags.commanded_go_around = true; + return true; +} + +bool AP_Landing::type_deepstall_is_throttle_suppressed(void) const +{ + return type_deepstall_stage == DEEPSTALL_STAGE_LAND; +} + +bool AP_Landing::type_deepstall_get_target_altitude_location(Location &location) +{ + memcpy(&location, &type_deepstall_landing_point, sizeof(Location)); + return true; +} + +int32_t AP_Landing::type_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; + } else { + return aparm.airspeed_cruise_cm; + } +} + +const DataFlash_Class::PID_Info& AP_Landing::type_deepstall_get_pid_info(void) const +{ + return type_deepstall_PID.get_pid_info(); +} + +void AP_Landing::type_deepstall_build_approach_path(void) +{ + Vector3f wind = ahrs.wind_estimate(); + // TODO: Support a user defined approach heading + type_deepstall_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)); + + //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); + + 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; + // an approach extension of 0 can result in a divide by 0 + if (fabsf(approach_extension) < 1.0f) { + approach_extension = 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)); + + // 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); + +#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)); + 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)); + 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)); + 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)expected_travel_distance); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)type_deepstall_target_heading_deg); +#endif // DEBUG_PRINTS + +} + +float AP_Landing::type_deepstall_predict_travel_distance(const Vector3f wind, const float height) const +{ + bool reverse = false; + + float course = radians(type_deepstall_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); + + 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 + Vector2f course_vec(cosf(course), sinf(course)); + + 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 theta = acosf(constrain_float((wind_vec * course_vec) / wind_length, -1.0f, 1.0f)); + if ((course_vec % wind_vec) > 0) { + reverse = true; + theta *= -1; + } + + float cross_component = sinf(theta) * wind_length; + float estimated_crab_angle = asinf(constrain_float(cross_component / forward_speed, -1.0f, 1.0f)); + if (reverse) { + estimated_crab_angle *= -1; + } + + float estimated_forward = cosf(estimated_crab_angle) * forward_speed + 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); +#endif // DEBUG_PRINTS + + return estimated_forward * height / type_deepstall_down_speed + stall_distance; +} + +bool AP_Landing::type_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)); + + // 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 + + if (heading_error <= 10.0 && fabsf(height_error) < DEEPSTALL_LOITER_ALT_TOLERANCE) { + // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp + return true; + } + return false; +} + +float AP_Landing::type_deepstall_update_steering() +{ + Location current_loc; + if (!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 + // 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)); + } + 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; + + + Vector2f ab = location_diff(type_deepstall_arc_exit, type_deepstall_extended_approach); + ab.normalize(); + Vector2f a_air = location_diff(type_deepstall_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 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; + } + + float desired_change = wrap_PI(radians(type_deepstall_target_heading_deg) + nu1 - 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); + +#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()); +#endif // DEBUG_PRINTS + + return type_deepstall_PID.get_pid(error); +}