From 93c6f64a91a792c7fb3b420025a9fbae1ce9f318 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 9 Mar 2017 12:25:54 -0500 Subject: [PATCH] Sub: Clean out/remove references to old landing code --- ArduSub/APM_Config.h | 2 -- ArduSub/GCS_Mavlink.cpp | 11 +++++----- ArduSub/Parameters.cpp | 4 ++-- ArduSub/Sub.h | 5 +---- ArduSub/commands_logic.cpp | 3 +-- ArduSub/config.h | 43 ++------------------------------------ ArduSub/control_auto.cpp | 13 ++---------- ArduSub/defines.h | 12 +++-------- ArduSub/switches.cpp | 13 ------------ 9 files changed, 16 insertions(+), 90 deletions(-) diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index a43c3957f8..95e2b82e49 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -25,8 +25,6 @@ // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) -//#define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED // when set to ENABLED vehicle will only disarm after landing (in AUTO, LAND or RTL) if pilot has put throttle to zero - //#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation // User Hooks : For User Developed code that you wish to run diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index b951d62a59..5908075a9a 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1174,12 +1174,11 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) } break; - // Not supported in sub - // case MAV_CMD_NAV_LAND: - // if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { - // result = MAV_RESULT_ACCEPTED; - // } - // break; + case MAV_CMD_NAV_LAND: + if (sub.set_mode(SURFACE, MODE_REASON_GCS_COMMAND)) { + result = MAV_RESULT_ACCEPTED; + } + break; case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 3d569e6471..031e4a9dba 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -838,8 +838,8 @@ const AP_Param::Info Sub::var_info[] = { // @Param: TERRAIN_FOLLOW // @DisplayName: Terrain Following use control - // @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. - // @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land + // @Description: This enables terrain following for RTL and SURFACE flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. + // @Values: 0:Do Not Use in RTL and SURFACE,1:Use in RTL and SURFACE // @User: Standard GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index bdccb9f66a..809bd78d68 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -249,7 +249,6 @@ private: enum HomeState home_state : 2; // home status (unset, set, locked) uint8_t using_interlock : 1; // aux switch motor interlock function is in use uint8_t motor_emergency_stop: 1; // motor estop switch, shuts off motors when enabled - uint8_t land_repo_active : 1; // true if the pilot is overriding the landing position uint8_t at_bottom : 1; // true if we are at the bottom uint8_t at_surface : 1; // true if we are at the surface uint8_t depth_sensor_present: 1; // true if we have an external baro connected @@ -342,7 +341,7 @@ private: uint32_t loiter_time; // How long have we been loitering - The start time in millis // Delay the next navigation command - int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.) + int32_t nav_delay_time_max; // used for delaying the navigation commands uint32_t nav_delay_time_start; // Battery Sensors @@ -585,7 +584,6 @@ private: bool far_from_EKF_origin(const Location& loc); void set_system_time_from_GPS(); void exit_mission(); - bool verify_land(); bool verify_loiter_unlimited(); bool verify_loiter_time(); bool verify_wait_delay(); @@ -637,7 +635,6 @@ private: void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); void guided_limit_init_time_and_pos(); bool guided_limit_check(); - float get_land_descent_speed(); bool velhold_init(bool ignore_checks); void velhold_run(); bool poshold_init(bool ignore_checks); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index b584d11548..99225839c1 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -569,7 +569,7 @@ bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } } -// verify_land - returns true if landing has been completed +// verify_surface - returns true if surface procedure has been completed bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd) { bool retval = false; @@ -590,7 +590,6 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd) break; case AUTO_SURFACE_STATE_ASCEND: - // rely on THROTTLE_LAND mode to correctly update landing status if (wp_nav.reached_wp_destination()) { retval = true; } diff --git a/ArduSub/config.h b/ArduSub/config.h index dfce140023..b5e507ca55 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -225,7 +225,7 @@ ////////////////////////////////////////////////////////////////////////////// // EKF Failsafe #ifndef FS_EKF_ACTION_DEFAULT -# define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_DISABLED // EKF failsafe triggers land by default +# define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_DISABLED // EKF failsafe #endif #ifndef FS_EKF_THRESHOLD_DEFAULT # define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered @@ -312,45 +312,6 @@ # define FS_THR_VALUE_DEFAULT 975 #endif - -////////////////////////////////////////////////////////////////////////////// -// Landing -// -#ifndef LAND_SPEED -# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s -#endif -#ifndef LAND_START_ALT -# define LAND_START_ALT -25 // altitude in cm where land controller switches to slow rate of descent -#endif -#ifndef LAND_REQUIRE_MIN_THROTTLE_TO_DISARM -# define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM DISABLED // we do not require pilot to reduce throttle to minimum before vehicle will disarm in AUTO, LAND or RTL -#endif -#ifndef LAND_REPOSITION_DEFAULT -# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing -#endif -#ifndef LAND_WITH_DELAY_MS -# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event -#endif -#ifndef LAND_CANCEL_TRIGGER_THR -# define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Landing Detector -// -#ifndef LAND_DETECTOR_TRIGGER_SEC -# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing -#endif -#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC -# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over) -#endif -#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF -# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter -#endif -#ifndef LAND_DETECTOR_ACCEL_MAX -# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s -#endif - ////////////////////////////////////////////////////////////////////////////// // CAMERA TRIGGER AND CONTROL // @@ -397,7 +358,7 @@ // RTL Mode #ifndef RTL_ALT_FINAL -# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. #endif #ifndef RTL_ALT diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 3b6bb20a8f..d067d529c8 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -1,18 +1,9 @@ #include "Sub.h" /* - * control_auto.pde - init and run calls for auto flight mode + * control_auto.cpp + * Contains the mission, waypoint navigation and NAV_CMD item implementation * - * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode - * Command execution code (i.e. command_logic.pde) should: - * a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called - * b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start() - * c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed - * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call - * correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature - */ - -/* * While in the auto flight mode, navigation or do/now commands can be run. * Code in this file implements the navigation commands */ diff --git a/ArduSub/defines.h b/ArduSub/defines.h index d2219b7ab6..89a64cbcc4 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -62,7 +62,6 @@ enum aux_sw_func { // AUXSW_SPRAYER = 15, // enable/disable the crop sprayer AUXSW_AUTO = 16, // change to auto flight mode - AUXSW_LAND = 18, // change to LAND flight mode AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on // No parachute for Sub, remove @@ -76,9 +75,6 @@ enum aux_sw_func { AUXSW_RETRACT_MOUNT = 27, // Retract Mount AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay) - // No landing gear for sub, remove - // AUXSW_LANDING_GEAR = 29, // Landing gear controller - AUXSW_LOST_VEHICLE_SOUND = 30, // Play lost vehicle sound AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch @@ -116,7 +112,7 @@ enum mode_reason_t { MODE_REASON_EKF_FAILSAFE, MODE_REASON_GPS_GLITCH, MODE_REASON_MISSION_END, - MODE_REASON_THROTTLE_LAND_ESCAPE, + MODE_REASON_THROTTLE_SURFACE_ESCAPE, MODE_REASON_FENCE_BREACH, MODE_REASON_TERRAIN_FAILSAFE, MODE_REASON_SURFACE_COMPLETE, @@ -145,7 +141,6 @@ enum mode_reason_t { // Auto modes enum AutoMode { Auto_WP, - Auto_Land, Auto_CircleMoveToEdge, Auto_Circle, Auto_Spline, @@ -218,7 +213,6 @@ enum RTLState { #define DATA_ARMED 10 #define DATA_DISARMED 11 #define DATA_AUTO_ARMED 15 -#define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 #define DATA_SET_HOME 25 #define DATA_SET_SIMPLE_ON 26 @@ -238,7 +232,7 @@ enum RTLState { #define DATA_MOTORS_INTERLOCK_DISABLED 56 #define DATA_MOTORS_INTERLOCK_ENABLED 57 #define DATA_EKF_ALT_RESET 60 -#define DATA_LAND_CANCELLED_BY_PILOT 61 +#define DATA_SURFACE_CANCELLED_BY_PILOT 61 #define DATA_EKF_YAW_RESET 62 #define DATA_SURFACED 63 #define DATA_NOT_SURFACED 64 @@ -301,7 +295,7 @@ enum RTLState { // Battery failsafe definitions (FS_BATT_ENABLE parameter) #define FS_BATT_DISABLED 0 // battery failsafe disabled -#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe +#define FS_BATT_SURFACE 1 // switch to SURFACE mode on battery failsafe #define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe // GCS failsafe definitions (FS_GCS_ENABLE parameter) diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index 0e97fa6e17..150a0f7484 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -280,19 +280,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; - case AUXSW_LAND: - // Do nothing for Sub - - // if (ch_flag == AUX_SWITCH_HIGH) { - // set_mode(LAND, MODE_REASON_TX_COMMAND); - // }else{ - // // return to flight mode switch's flight mode if we are currently in LAND - // if (control_mode == LAND) { - // reset_control_switch(); - // } - // } - break; - case AUXSW_MISSION_RESET: if (ch_flag == AUX_SWITCH_HIGH) { mission.reset();