From 9ec79ab6544de3e21c37076cec41e9e864fbf331 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 7 Dec 2016 14:03:53 -0500 Subject: [PATCH] Sub: Remove RC radio failsafe --- ArduSub/AP_State.cpp | 27 --------------------------- ArduSub/Parameters.cpp | 16 ---------------- ArduSub/Parameters.h | 4 ---- ArduSub/Sub.h | 6 ------ ArduSub/compassmot.cpp | 4 +--- ArduSub/events.cpp | 42 ------------------------------------------ ArduSub/motor_test.cpp | 2 -- ArduSub/switches.cpp | 2 +- 8 files changed, 2 insertions(+), 101 deletions(-) diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index d8a60855ec..f4b80f770a 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -57,33 +57,6 @@ void Sub::set_simple_mode(uint8_t b) } } -// --------------------------------------------- -void Sub::set_failsafe_radio(bool b) -{ - // only act on changes - // ------------------- - if(failsafe.radio != b) { - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe.radio = b; - - if (failsafe.radio == false) { - // We've regained radio contact - // ---------------------------- - failsafe_radio_off_event(); - }else{ - // We've lost radio contact - // ------------------------ - failsafe_radio_on_event(); - } - - // update AP_Notify - AP_Notify::flags.failsafe_radio = b; - } -} - - // --------------------------------------------- void Sub::set_failsafe_battery(bool b) { diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index a713b75d54..927a1fe61a 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -222,22 +222,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), - // @Param: FS_THR_ENABLE - // @DisplayName: Throttle Failsafe Enable - // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always LAND - // @User: Standard - GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_DISABLED), - - // @Param: FS_THR_VALUE - // @DisplayName: Throttle Failsafe Value - // @Description: The PWM level on channel 3 below which throttle sailsafe triggers - // @Range: 925 1100 - // @Units: pwm - // @Increment: 1 - // @User: Standard - GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), - // @Param: THR_DZ // @DisplayName: Throttle deadzone // @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index fdae4d847d..c72775ee42 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -182,8 +182,6 @@ public: k_param_failsafe_battery_enabled, k_param_fs_batt_mah, k_param_fs_batt_voltage, - k_param_failsafe_throttle, - k_param_failsafe_throttle_value, // Misc Sub settings @@ -286,8 +284,6 @@ public: // Throttle // - AP_Int8 failsafe_throttle; - AP_Int16 failsafe_throttle_value; AP_Int16 throttle_deadzone; // Flight modes diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 3334ce54e7..2d7c5b8620 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -288,8 +288,6 @@ private: uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs uint32_t last_gcs_warn_ms; - int8_t radio_counter; // number of iterations with throttle below throttle_fs_value - uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed @@ -522,7 +520,6 @@ private: bool home_is_set(); void set_auto_armed(bool b); void set_simple_mode(uint8_t b); - void set_failsafe_radio(bool b); void set_failsafe_battery(bool b); void set_pre_arm_check(bool b); void set_pre_arm_rc_check(bool b); @@ -708,8 +705,6 @@ private: void esc_calibration_passthrough(); void esc_calibration_auto(); bool should_disarm_on_failsafe(); - void failsafe_radio_on_event(); - void failsafe_radio_off_event(); void failsafe_battery_event(void); void failsafe_gcs_check(); void failsafe_terrain_check(); @@ -784,7 +779,6 @@ private: void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); void camera_tilt_smooth(); JSButton* get_button(uint8_t index); - void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); void radio_passthrough_to_motors(); void init_barometer(bool full_calibration); diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp index 165624db4d..fca31942ba 100644 --- a/ArduSub/compassmot.cpp +++ b/ArduSub/compassmot.cpp @@ -98,8 +98,7 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); } - // disable throttle and battery failsafe - g.failsafe_throttle = FS_THR_DISABLED; + // disable battery failsafe g.failsafe_battery_enabled = FS_BATT_DISABLED; // disable motor compensation @@ -256,7 +255,6 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) failsafe_enable(); // re-enable failsafes - g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); // flag we have completed diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index 3d48ae8b9e..9157079e64 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -2,48 +2,6 @@ #include "Sub.h" -/* - * This event will be called when the failsafe changes - * boolean failsafe reflects the current state - */ -void Sub::failsafe_radio_on_event() -{ -// // if motors are not armed there is nothing to do -// if( !motors.armed() ) { -// return; -// } -// -// if (should_disarm_on_failsafe()) { -// init_disarm_motors(); -// } else { -// if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) { -// // continue mission -// } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) { -// // continue landing -// } else { -// if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { -// set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE); -// } else { -// set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE); -// } -// } -// } -// -// // log the error to the dataflash -// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED); - -} - -// failsafe_off_event - respond to radio contact being regained -// we must be in AUTO, LAND or RTL modes -// or Stabilize or ACRO mode but with motors disarmed -void Sub::failsafe_radio_off_event() -{ - // no need to do anything except log the error as resolved - // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED); -} - void Sub::failsafe_battery_event(void) { // // return immediately if low battery event has already been triggered diff --git a/ArduSub/motor_test.cpp b/ArduSub/motor_test.cpp index 07d0b2118f..844a72b130 100644 --- a/ArduSub/motor_test.cpp +++ b/ArduSub/motor_test.cpp @@ -112,7 +112,6 @@ uint8_t Sub::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, } // disable throttle, battery and gps failsafe - g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED; g.failsafe_gcs = FS_GCS_DISABLED; @@ -153,7 +152,6 @@ void Sub::motor_test_stop() motor_test_timeout_ms = 0; // re-enable failsafes - g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); g.failsafe_gcs.load(); diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index 514e155462..4c0601b574 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -125,7 +125,7 @@ void Sub::read_aux_switches() uint8_t switch_position; // exit immediately during radio failsafe - if (failsafe.radio || failsafe.radio_counter != 0) { + if (failsafe.radio) { return; }