AP_Landing: restructure parameters into a structure
This commit is contained in:
Andrew Tridgell
parent
041fe38ba6
commit
e7024d9203
@ -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,7 +206,7 @@ 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,
|
||||
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:
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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&);
|
||||
@ -42,22 +43,7 @@ public:
|
||||
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);
|
||||
}
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
@ -22,140 +22,250 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
108
libraries/AP_Landing/AP_Landing_Deepstall.h
Normal file
108
libraries/AP_Landing/AP_Landing_Deepstall.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Mission/AP_Mission.h>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
|
||||
#include <AP_Navigation/AP_Navigation.h>
|
||||
#include <PID/PID.h>
|
||||
|
||||
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
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user