From 7b5d209aba76f12d9938351d5f06a710f980419e Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 25 Nov 2016 14:30:30 -0500 Subject: [PATCH] Sub: Remove parachute --- ArduSub/APM_Config.h | 1 - ArduSub/GCS_Mavlink.cpp | 24 ------------------------ ArduSub/Parameters.cpp | 6 ------ ArduSub/Parameters.h | 3 --- ArduSub/Sub.cpp | 3 --- ArduSub/Sub.h | 14 -------------- ArduSub/commands_logic.cpp | 31 ------------------------------- ArduSub/config.h | 6 ------ ArduSub/defines.h | 20 ++++++++++---------- ArduSub/make.inc | 1 - ArduSub/switches.cpp | 33 --------------------------------- 11 files changed, 10 insertions(+), 132 deletions(-) diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index 38c83e0713..ad2ce35da9 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -25,7 +25,6 @@ //#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash //#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally) #define AC_TERRAIN DISABLED // disable terrain library -#define PARACHUTE DISABLED // disable parachute release to save 1k of flash #define GRIPPER_ENABLED DISABLED // disable gripper to save 500bytes of flash //#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index d8cafa31a3..ba418b5364 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1546,30 +1546,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif break; -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: - // configure or release parachute - result = MAV_RESULT_ACCEPTED; - switch ((uint16_t)packet.param1) { - case PARACHUTE_DISABLE: - sub.parachute.enabled(false); - sub.Log_Write_Event(DATA_PARACHUTE_DISABLED); - break; - case PARACHUTE_ENABLE: - sub.parachute.enabled(true); - sub.Log_Write_Event(DATA_PARACHUTE_ENABLED); - break; - case PARACHUTE_RELEASE: - // treat as a manual release which performs some additional check of altitude - sub.parachute_manual_release(); - break; - default: - result = MAV_RESULT_FAILED; - break; - } - break; -#endif - case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index fb49c7ec8e..1fbc9d6c8e 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -860,12 +860,6 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), -#if PARACHUTE == ENABLED - // @Group: CHUTE_ - // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp - GOBJECT(parachute, "CHUTE_", AP_Parachute), -#endif - // @Group: LGR_ // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp GOBJECT(landinggear, "LGR_", AP_LandingGear), diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 28001fdedd..7a9a03fdd7 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -73,9 +73,6 @@ public: // GPS object k_param_gps, - // Parachute object - k_param_parachute, - // Landing gear object k_param_landinggear, // 18 diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 35d3be1373..aee189f231 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -91,9 +91,6 @@ Sub::Sub(void) : #if SPRAYER == ENABLED sprayer(&inertial_nav), #endif -#if PARACHUTE == ENABLED - parachute(relay), -#endif #if AP_TERRAIN_AVAILABLE && AC_TERRAIN terrain(ahrs, mission, rally), #endif diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 369ee1e525..8f36487ee2 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -105,9 +105,6 @@ #if GRIPPER_ENABLED == ENABLED #include // gripper stuff #endif -#if PARACHUTE == ENABLED -#include // Parachute release library -#endif #if PRECISION_LANDING == ENABLED #include #include @@ -500,11 +497,6 @@ private: AC_Sprayer sprayer; #endif - // Parachute release -#if PARACHUTE == ENABLED - AP_Parachute parachute; -#endif - // Landing Gear Controller AP_LandingGear landinggear; @@ -766,9 +758,6 @@ private: bool manual_init(bool ignore_checks); void manual_run(); void crash_check(); - void parachute_check(); - void parachute_release(); - void parachute_manual_release(); void ekf_check(); bool ekf_over_threshold(); void failsafe_ekf_event(); @@ -936,9 +925,6 @@ private: void do_digicam_configure(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd); #endif -#if PARACHUTE == ENABLED - void do_parachute(const AP_Mission::Mission_Command& cmd); -#endif #if GRIPPER_ENABLED == ENABLED void do_gripper(const AP_Mission::Mission_Command& cmd); #endif diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 73ba0d33e3..d05cec14ea 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -124,12 +124,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) break; #endif -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute - do_parachute(cmd); - break; -#endif - #if GRIPPER_ENABLED == ENABLED case MAV_CMD_DO_GRIPPER: // Mission command to control gripper do_gripper(cmd); @@ -234,7 +228,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - case MAV_CMD_DO_PARACHUTE: // assume parachute was released successfully case MAV_CMD_DO_GRIPPER: case MAV_CMD_DO_GUIDED_LIMITS: return true; @@ -529,30 +522,6 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(nav_delay_time_max/1000)); } - -#if PARACHUTE == ENABLED -// do_parachute - configure or release parachute -void Sub::do_parachute(const AP_Mission::Mission_Command& cmd) -{ - switch (cmd.p1) { - case PARACHUTE_DISABLE: - parachute.enabled(false); - Log_Write_Event(DATA_PARACHUTE_DISABLED); - break; - case PARACHUTE_ENABLE: - parachute.enabled(true); - Log_Write_Event(DATA_PARACHUTE_ENABLED); - break; - case PARACHUTE_RELEASE: - parachute_release(); - break; - default: - // do nothing - break; - } -} -#endif - #if GRIPPER_ENABLED == ENABLED // do_gripper - control gripper void Sub::do_gripper(const AP_Mission::Mission_Command& cmd) diff --git a/ArduSub/config.h b/ArduSub/config.h index 73cbba17ff..838ce89aa5 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -255,12 +255,6 @@ # define EPM_ENABLED ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Parachute release -#ifndef PARACHUTE - # define PARACHUTE ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // Nav-Guided - allows external nav computer to control vehicle #ifndef NAV_GUIDED diff --git a/ArduSub/defines.h b/ArduSub/defines.h index d1f64732fd..f6ee42c872 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -56,9 +56,12 @@ enum aux_sw_func { AUXSW_AUTOTUNE = 17, // auto tune AUXSW_LAND = 18, // change to LAND flight mode AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on - AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable - AUXSW_PARACHUTE_RELEASE = 22, // Parachute release - AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch + +// No parachute for Sub, remove +// AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable +// AUXSW_PARACHUTE_RELEASE = 22, // Parachute release +// AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch + AUXSW_MISSION_RESET = 24, // Reset auto mission to start from first command AUXSW_ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting @@ -340,9 +343,9 @@ enum ThrowModeState { #define DATA_ACRO_TRAINER_LIMITED 45 #define DATA_GRIPPER_GRAB 46 #define DATA_GRIPPER_RELEASE 47 -#define DATA_PARACHUTE_DISABLED 49 -#define DATA_PARACHUTE_ENABLED 50 -#define DATA_PARACHUTE_RELEASED 51 +//#define DATA_PARACHUTE_DISABLED 49 // Remove +//#define DATA_PARACHUTE_ENABLED 50 // Remove +//#define DATA_PARACHUTE_RELEASED 51 // Remove #define DATA_LANDING_GEAR_DEPLOYED 52 #define DATA_LANDING_GEAR_RETRACTED 53 #define DATA_MOTORS_EMERGENCY_STOPPED 54 @@ -377,7 +380,7 @@ enum ThrowModeState { #define ERROR_SUBSYSTEM_CRASH_CHECK 12 //#define ERROR_SUBSYSTEM_FLIP 13 #define ERROR_SUBSYSTEM_AUTOTUNE 14 -#define ERROR_SUBSYSTEM_PARACHUTE 15 +//#define ERROR_SUBSYSTEM_PARACHUTE 15 // Remove #define ERROR_SUBSYSTEM_EKFCHECK 16 #define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17 #define ERROR_SUBSYSTEM_BARO 18 @@ -411,9 +414,6 @@ enum ThrowModeState { #define ERROR_CODE_FAILED_CIRCLE_INIT 4 #define ERROR_CODE_DEST_OUTSIDE_FENCE 5 -// parachute failed to deploy because of low altitude or landed -#define ERROR_CODE_PARACHUTE_TOO_LOW 2 -#define ERROR_CODE_PARACHUTE_LANDED 3 // EKF check definitions #define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2 #define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0 diff --git a/ArduSub/make.inc b/ArduSub/make.inc index eb74b3ce39..b122c010f3 100644 --- a/ArduSub/make.inc +++ b/ArduSub/make.inc @@ -52,7 +52,6 @@ LIBRARIES += AP_BattMonitor LIBRARIES += AP_BoardConfig LIBRARIES += AP_Frsky_Telem LIBRARIES += AC_Sprayer -LIBRARIES += AP_Parachute LIBRARIES += AP_LandingGear LIBRARIES += AP_Terrain LIBRARIES += AP_RPM diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index 211ec73fa5..f9b4104655 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -235,8 +235,6 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_ACRO_TRAINER: case AUXSW_GRIPPER: case AUXSW_SPRAYER: - case AUXSW_PARACHUTE_ENABLE: - case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release case AUXSW_RETRACT_MOUNT: case AUXSW_MISSION_RESET: case AUXSW_ATTCON_FEEDFWD: @@ -441,37 +439,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // } break; -#if PARACHUTE == ENABLED - case AUXSW_PARACHUTE_ENABLE: - // Parachute enable/disable - parachute.enabled(ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_PARACHUTE_RELEASE: - if (ch_flag == AUX_SWITCH_HIGH) { - parachute_manual_release(); - } - break; - - case AUXSW_PARACHUTE_3POS: - // Parachute disable, enable, release with 3 position switch - switch (ch_flag) { - case AUX_SWITCH_LOW: - parachute.enabled(false); - Log_Write_Event(DATA_PARACHUTE_DISABLED); - break; - case AUX_SWITCH_MIDDLE: - parachute.enabled(true); - Log_Write_Event(DATA_PARACHUTE_ENABLED); - break; - case AUX_SWITCH_HIGH: - parachute.enabled(true); - parachute_manual_release(); - break; - } - break; -#endif - case AUXSW_MISSION_RESET: if (ch_flag == AUX_SWITCH_HIGH) { mission.reset();