diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 70c7f57eef..1985fec40b 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -110,7 +110,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(update_notify, 50, 90), SCHED_TASK(one_hz_loop, 1, 100), SCHED_TASK(ekf_check, 10, 75), - SCHED_TASK(landinggear_update, 10, 75), SCHED_TASK(lost_vehicle_check, 10, 50), SCHED_TASK(gcs_check_input, 400, 180), SCHED_TASK(gcs_send_heartbeat, 1, 110), diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 1fbc9d6c8e..7ad7a5e82d 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -860,10 +860,6 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), - // @Group: LGR_ - // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp - GOBJECT(landinggear, "LGR_", AP_LandingGear), - // @Group: COMPASS_ // @Path: ../libraries/AP_Compass/AP_Compass.cpp GOBJECT(compass, "COMPASS_", Compass), diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 7a9a03fdd7..33549c88d9 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -73,9 +73,6 @@ public: // GPS object k_param_gps, - // Landing gear object - k_param_landinggear, // 18 - // Input Management object k_param_input_manager, // 19 diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 8f36487ee2..d61514200f 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -86,7 +86,6 @@ #include // Notify library #include // Battery monitor library #include // board configuration library -#include // Landing Gear library #include #include #include @@ -497,9 +496,6 @@ private: AC_Sprayer sprayer; #endif - // Landing Gear Controller - AP_LandingGear landinggear; - // terrain handling #if AP_TERRAIN_AVAILABLE && AC_TERRAIN AP_Terrain terrain; @@ -792,7 +788,6 @@ private: void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); void set_bottomed(bool at_bottom); - void landinggear_update(); void update_notify(); void motor_test_output(); bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index f6ee42c872..f52c11ecf6 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -67,7 +67,10 @@ enum aux_sw_func { AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting AUXSW_RETRACT_MOUNT = 27, // Retract Mount AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay) - AUXSW_LANDING_GEAR = 29, // Landing gear controller + +// No landing gear for sub, remove +// AUXSW_LANDING_GEAR = 29, // Landing gear controller + AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch @@ -346,8 +349,8 @@ enum ThrowModeState { //#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_LANDING_GEAR_DEPLOYED 52 // Remove +//#define DATA_LANDING_GEAR_RETRACTED 53 // Remove #define DATA_MOTORS_EMERGENCY_STOPPED 54 #define DATA_MOTORS_EMERGENCY_STOP_CLEARED 55 #define DATA_MOTORS_INTERLOCK_DISABLED 56 diff --git a/ArduSub/landing_gear.cpp b/ArduSub/landing_gear.cpp deleted file mode 100644 index aa9b19a4db..0000000000 --- a/ArduSub/landing_gear.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - - -// Run landing gear controller at 10Hz -void Sub::landinggear_update(){ - - // Do nothing for Sub - - -// // If landing gear control is active, run update function. -// if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ -// -// // last status (deployed or retracted) used to check for changes -// static bool last_deploy_status; -// -// // if we are doing an automatic landing procedure, force the landing gear to deploy. -// // To-Do: should we pause the auto-land procedure to give time for gear to come down? -// if (control_mode == LAND || -// (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || -// (control_mode == AUTO && auto_mode == Auto_Land) || -// (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { -// landinggear.force_deploy(true); -// } -// -// landinggear.update(); -// -// // send event message to datalog if status has changed -// if (landinggear.deployed() != last_deploy_status){ -// if (landinggear.deployed()) { -// Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); -// } else { -// Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); -// } -// } -// -// last_deploy_status = landinggear.deployed(); -// } -} diff --git a/ArduSub/make.inc b/ArduSub/make.inc index b122c010f3..81b6929c27 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_LandingGear LIBRARIES += AP_Terrain LIBRARIES += AP_RPM LIBRARIES += AC_PrecLand diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index f9b4104655..535e399ccc 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -240,7 +240,6 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_ATTCON_FEEDFWD: case AUXSW_ATTCON_ACCEL_LIM: case AUXSW_RELAY: - case AUXSW_LANDING_GEAR: case AUXSW_MOTOR_ESTOP: case AUXSW_MOTOR_INTERLOCK: do_aux_switch_function(ch_option, ch_flag); @@ -472,20 +471,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); break; - case AUXSW_LANDING_GEAR: - switch (ch_flag) { - case AUX_SWITCH_LOW: - landinggear.set_cmd_mode(LandingGear_Deploy); - break; - case AUX_SWITCH_MIDDLE: - landinggear.set_cmd_mode(LandingGear_Auto); - break; - case AUX_SWITCH_HIGH: - landinggear.set_cmd_mode(LandingGear_Retract); - break; - } - break; - case AUXSW_LOST_COPTER_SOUND: switch (ch_flag) { case AUX_SWITCH_HIGH: diff --git a/ArduSub/wscript b/ArduSub/wscript index 74faa3684c..260d1e5001 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -20,7 +20,6 @@ def build(bld): 'AP_IRLock', 'AP_InertialNav', 'AP_JSButton', - 'AP_LandingGear', 'AP_LeakDetector', 'AP_Menu', 'AP_Motors',