Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove RTL

Browse Source
This commit is contained in:
Jacob Walser 2016-11-26 01:06:17 -05:00 committed by Andrew Tridgell
parent 23e460d71d
commit 0d575681de
12 changed files with 9 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduSub/GCS_Mavlink.cpp
View File

@ -48,7 +48,6 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
switch (control_mode) {
case AUTO:
case RTL:
case VELHOLD:
case GUIDED:
case CIRCLE:
@ -168,7 +167,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
case AUTO:
case GUIDED:
case VELHOLD:
case RTL:
case CIRCLE:
case SURFACE:
case OF_LOITER:
@ -1268,12 +1266,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (sub.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// Not supported in sub
// case MAV_CMD_NAV_LAND:
// if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {

54
ArduSub/Parameters.cpp
View File

@ -104,33 +104,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
// @Param: RTL_ALT
// @DisplayName: RTL Altitude
// @Description: The minimum relative altitude the model will move to before Returning to Launch. Set to zero to return at current altitude.
// @Units: Centimeters
// @Range: 0 8000
// @Increment: 1
// @User: Standard
GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT),
// @Param: RTL_CONE_SLOPE
// @DisplayName: RTL cone slope
// @Description: Defines a cone above home which determines maximum climb
// @Range: 0.5 10.0
// @Increment: .1
// @Values: 0:Disabled,1:Shallow,3:Steep
// @User: Standard
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT),
// @Param: RTL_SPEED
// @DisplayName: RTL speed
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
// @Units: cm/s
// @Range: 0 2000
// @Increment: 50
// @User: Standard
GSCALAR(rtl_speed_cms, "RTL_SPEED", 0),
// @Param: RNGFND_GAIN
// @DisplayName: Rangefinder gain
// @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter
@ -232,24 +205,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER),
// @Param: RTL_ALT_FINAL
// @DisplayName: RTL Final Altitude
// @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land.
// @Units: Centimeters
// @Range: -1 1000
// @Increment: 1
// @User: Standard
GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL),
// @Param: RTL_CLIMB_MIN
// @DisplayName: RTL minimum climb
// @Description: The vehicle will climb this many cm during the initial climb portion of the RTL
// @Units: Centimeters
// @Range: 0 3000
// @Increment: 10
// @User: Standard
GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT),
// @Param: WP_YAW_BEHAVIOR
// @DisplayName: Yaw behaviour during missions
// @Description: Determines how the autopilot controls the yaw during missions and RTL
@ -257,15 +212,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT),
// @Param: RTL_LOIT_TIME
// @DisplayName: RTL loiter time
// @Description: Time (in milliseconds) to loiter above home before beginning final descent
// @Units: ms
// @Range: 0 60000
// @Increment: 1000
// @User: Standard
GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME),
// @Param: PILOT_VELZ_MAX
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum vertical velocity the pilot may request in cm/s

21
ArduSub/Parameters.h
View File

@ -144,13 +144,6 @@ public:
k_param_gcs3,
k_param_gcs_pid_mask, // 126
//
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
//
// 140: Sensor parameters
//
@ -160,13 +153,6 @@ public:
k_param_ch7_option,
k_param_ahrs, // AHRS group // 159
//
// 160: Navigation parameters
//
k_param_rtl_altitude = 160,
k_param_rtl_loiter_time,
k_param_rtl_alt_final,
//
// Camera and mount parameters
//
@ -224,7 +210,6 @@ public:
k_param_autotune_aggressiveness,
k_param_pi_vel_xy,
k_param_fs_ekf_action,
k_param_rtl_climb_min,
k_param_rpm_sensor,
k_param_autotune_min_d, // 251
k_param_DataFlash = 253, // 253 - Logging Group
@ -290,9 +275,6 @@ public:
AP_Float throttle_filt;
AP_Int16 throttle_behavior;
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float rangefinder_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
@ -312,15 +294,12 @@ public:
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position
AP_Int8 compass_enabled;
AP_Int16 rtl_alt_final;
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
// Waypoints
//
AP_Int32 rtl_loiter_time;
AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request
AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request

3
ArduSub/Sub.cpp
View File

@ -36,8 +36,6 @@ Sub::Sub(void) :
wp_distance(0),
auto_mode(Auto_WP),
guided_mode(Guided_WP),
rtl_state(RTL_InitialClimb),
rtl_state_complete(false),
circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),
@ -73,7 +71,6 @@ Sub::Sub(void) :
pmTest1(0),
fast_loopTimer(0),
mainLoop_count(0),
rtl_loiter_start_time(0),
auto_trim_counter(0),
ServoRelayEvents(relay),
#if CAMERA == ENABLED

34
ArduSub/Sub.h
View File

@ -331,20 +331,6 @@ private:
// Guided
GuidedMode guided_mode; // controls which controller is run (pos or vel)
// RTL
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc)
bool rtl_state_complete; // set to true if the current state is completed
struct {
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
Location_Class origin_point;
Location_Class climb_target;
Location_Class return_target;
Location_Class descent_target;
bool land;
bool terrain_used;
} rtl_path;
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@ -444,8 +430,6 @@ private:
uint32_t fast_loopTimer;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
uint16_t mainLoop_count;
// Loiter timer - Records how long we have been in loiter
uint32_t rtl_loiter_start_time;
// Used to exit the roll and pitch auto trim function
uint8_t auto_trim_counter;
@ -614,11 +598,9 @@ private:
bool far_from_EKF_origin(const Location& loc);
void set_system_time_from_GPS();
void exit_mission();
void do_RTL(void);
bool verify_land();
bool verify_loiter_unlimited();
bool verify_loiter_time();
bool verify_RTL();
bool verify_wait_delay();
bool verify_within_distance();
bool verify_yaw();
@ -639,8 +621,6 @@ private:
void auto_land_start();
void auto_land_start(const Vector3f& destination);
void auto_land_run();
void auto_rtl_start();
void auto_rtl_run();
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
void auto_circle_start();
void auto_circle_run();
@ -704,20 +684,6 @@ private:
bool poshold_init(bool ignore_checks);
void poshold_run();
bool rtl_init(bool ignore_checks);
void rtl_restart_without_terrain();
void rtl_run();
void rtl_climb_start();
void rtl_return_start();
void rtl_climb_return_run();
void rtl_loiterathome_start();
void rtl_loiterathome_run();
void rtl_descent_start();
void rtl_descent_run();
void rtl_land_start();
void rtl_land_run();
void rtl_build_path(bool terrain_following_allowed);
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed);
bool transect_init(bool ignore_checks);
void transect_run();
bool stabilize_init(bool ignore_checks);

7
ArduSub/arming_checks.cpp
View File

@ -482,13 +482,6 @@ bool Sub::pre_arm_terrain_check(bool display_failure)
return true;
}
// check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
// To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
return false;
}
// show terrain statistics
uint16_t terr_pending, terr_loaded;
terrain.get_statistics(terr_pending, terr_loaded);

26
ArduSub/commands_logic.cpp
View File

@ -35,10 +35,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20
do_RTL();
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
@ -189,9 +185,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_SPLINE_WAYPOINT:
return verify_spline_wp(cmd);
@ -252,17 +245,6 @@ void Sub::exit_mission()
}
}
/********************************************************************************/
//
/********************************************************************************/
// do_RTL - start Return-to-Launch
void Sub::do_RTL(void)
{
// start rtl in auto flight mode
auto_rtl_start();
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
@ -635,14 +617,6 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
bool Sub::verify_RTL()
{
return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
}
// verify_spline_wp - check if we have reached the next way point using spline
bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{

21
ArduSub/control_auto.cpp
View File

@ -66,10 +66,6 @@ void Sub::auto_run()
auto_land_run();
break;
case Auto_RTL:
auto_rtl_run();
break;
case Auto_Circle:
auto_circle_run();
break;
@ -437,23 +433,6 @@ void Sub::auto_land_run()
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// auto_rtl_start - initialises RTL in AUTO flight mode
void Sub::auto_rtl_start()
{
auto_mode = Auto_RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks
rtl_init(true);
}
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void Sub::auto_rtl_run()
{
// call regular rtl flight mode run function
rtl_run();
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks

9
ArduSub/defines.h
View File

@ -42,7 +42,10 @@ enum aux_sw_func {
AUXSW_DO_NOTHING = 0, // aux switch disabled
// AUXSW_FLIP = 2, // flip
AUXSW_SIMPLE_MODE = 3, // change to simple mode
AUXSW_RTL = 4, // change to RTL flight mode
// No RTL mode for Sub
// AUXSW_RTL = 4, // change to RTL flight mode
AUXSW_SAVE_TRIM = 5, // save current position as level
AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay
@ -112,7 +115,7 @@ enum control_mode_t {
AUTO = 3, // not implemented in sub // fully automatic waypoint control using mission commands
GUIDED = 4, // not implemented in sub // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
VELHOLD = 5, // automatic x/y velocity control and automatic depth/throttle
RTL = 6, // not implemented in sub // automatic return to launching point
// RTL = 6, // not implemented in sub // automatic return to launching point
CIRCLE = 7, // not implemented in sub // automatic circular flight with automatic throttle
SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
OF_LOITER = 10, // deprecated
@ -207,7 +210,7 @@ enum AutoMode {
Auto_TakeOff,
Auto_WP,
Auto_Land,
Auto_RTL,
// Auto_RTL,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline,

2
ArduSub/events.cpp
View File

@ -220,7 +220,7 @@ void Sub::failsafe_gcs_check()
void Sub::failsafe_terrain_check()
{
// trigger with 5 seconds of failures while in AUTO mode
bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL);
bool valid_mode = (control_mode == AUTO || control_mode == GUIDED);
bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
bool trigger_event = valid_mode && timeout;

13
ArduSub/flight_mode.cpp
View File

@ -59,10 +59,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
success = surface_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
#if TRANSECT_ENABLED == ENABLED
case TRANSECT:
success = transect_init(ignore_checks);
@ -162,10 +158,6 @@ void Sub::update_flight_mode()
surface_run();
break;
case RTL:
rtl_run();
break;
#if TRANSECT_ENABLED == ENABLED
case TRANSECT:
transect_run();
@ -226,7 +218,6 @@ bool Sub::mode_requires_GPS(control_mode_t mode) {
case AUTO:
case GUIDED:
case VELHOLD:
case RTL:
case CIRCLE:
case POSHOLD:
case TRANSECT:
@ -266,7 +257,6 @@ void Sub::notify_flight_mode(control_mode_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case RTL:
case CIRCLE:
case SURFACE:
// autopilot modes
@ -303,9 +293,6 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case VELHOLD:
port->print("VELHOLD");
break;
case RTL:
port->print("RTL");
break;
case CIRCLE:
port->print("CIRCLE");
break;

4
ArduSub/navigation.cpp
View File

@ -28,7 +28,7 @@ void Sub::calc_wp_distance()
// get target from loiter or wpinav controller
if (control_mode == CIRCLE) {
wp_distance = wp_nav.get_loiter_distance_to_target();
}else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
}else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_distance = wp_nav.get_wp_distance_to_destination();
}else{
wp_distance = 0;
@ -41,7 +41,7 @@ void Sub::calc_wp_bearing()
// get target from loiter or wpinav controller
if (control_mode == CIRCLE) {
wp_bearing = wp_nav.get_loiter_bearing_to_target();
} else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
} else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_bearing = wp_nav.get_wp_bearing_to_destination();
} else {
wp_bearing = 0;