diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 94570dda2c..8d52e5be6d 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -293,6 +293,7 @@ void Sub::rc_loop() // Read radio // ----------------------------------------- read_radio(); + failsafe_manual_control_check(); } // throttle_loop - should be run at 50 hz diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index e9abcc9f71..a08406afe4 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -163,7 +163,7 @@ float Sub::get_pilot_desired_throttle(int16_t throttle_control) float Sub::get_pilot_desired_climb_rate(float throttle_control) { // throttle failsafe check - if( failsafe.radio ) { + if( failsafe.manual_control ) { return 0.0f; } diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 16476e8378..ed78b816b5 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -105,6 +105,7 @@ Sub::Sub(void) : sensor_health.compass = true; failsafe.last_heartbeat_ms = 0; + failsafe.manual_control = true; } Sub sub; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 5c931a21f1..595973fd0d 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -279,7 +279,7 @@ private: // Failsafe struct { uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station - uint8_t radio : 1; // 1 // A status flag for the radio failsafe + uint8_t manual_control : 1; // 1 // A status flag for the radio failsafe uint8_t battery : 1; // 2 // A status flag for the battery failsafe uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred @@ -291,6 +291,7 @@ private: uint32_t last_gcs_warn_ms; uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + uint32_t last_manual_control_ms; // last time MANUAL_CONTROL message arrived from GCS 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 } failsafe; @@ -694,6 +695,8 @@ private: bool should_disarm_on_failsafe(); void failsafe_battery_event(void); void failsafe_gcs_check(); + void failsafe_manual_control_check(void); + void set_neutral_controls(void); void failsafe_terrain_check(); void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_on_event(); diff --git a/ArduSub/arming_checks.cpp b/ArduSub/arming_checks.cpp index 0eaa378e85..848c42e273 100644 --- a/ArduSub/arming_checks.cpp +++ b/ArduSub/arming_checks.cpp @@ -22,6 +22,10 @@ void Sub::update_arming_checks(void) // performs pre-arm checks and arming checks bool Sub::all_arming_checks_passing(bool arming_from_gcs) { + if(failsafe.manual_control) { + gcs_send_text(MAV_SEVERITY_WARNING, "Arming requires manual control"); + return false; + } if (pre_arm_checks(true)) { set_pre_arm_check(true); } diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index d6ff76d169..a9cc42a768 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -55,7 +55,7 @@ void Sub::circle_run() } // process pilot inputs - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index ccee6e332d..6a0639c658 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -358,7 +358,7 @@ void Sub::guided_pos_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { @@ -402,7 +402,7 @@ void Sub::guided_vel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { @@ -451,7 +451,7 @@ void Sub::guided_posvel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!failsafe.manual_control) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { diff --git a/ArduSub/ekf_check.cpp b/ArduSub/ekf_check.cpp index 446d040c0b..d0dd5a304d 100644 --- a/ArduSub/ekf_check.cpp +++ b/ArduSub/ekf_check.cpp @@ -141,7 +141,7 @@ void Sub::failsafe_ekf_event() // switch (g.fs_ekf_action) { // case FS_EKF_ACTION_ALTHOLD: // // AltHold -// if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { +// if (failsafe.manual_control || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) { // set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE); // } // break; diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index 2e5fcbc2e4..48d9c26048 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -31,6 +31,23 @@ void Sub::failsafe_battery_event(void) } +void Sub::failsafe_manual_control_check() { + uint32_t tnow = AP_HAL::millis(); + + // Require at least 2Hz update + if(tnow > failsafe.last_manual_control_ms + 500) { + if(!failsafe.manual_control) { + failsafe.manual_control = true; + set_neutral_controls(); + init_disarm_motors(); + gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control"); + } + return; + } + + failsafe.manual_control = false; +} + void Sub::failsafe_internal_pressure_check() { if(g.failsafe_pressure == FS_PRESS_DISABLED) { diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 3370fdf8f8..9661e84a32 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -32,7 +32,7 @@ void Sub::fence_check() // // // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle // // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down -// if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ +// if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ // init_disarm_motors(); // }else{ // // if we are within 100m of the fence, RTL diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 0fac559d5f..07d1e698c7 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -43,6 +43,7 @@ void Sub::init_joystick() { } void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) { + int16_t channels[11]; uint32_t tnow_ms = millis(); @@ -397,6 +398,9 @@ JSButton* Sub::get_button(uint8_t index) { } void Sub::camera_tilt_smooth() { + if(failsafe.manual_control) { + return; + } int16_t channels[11]; for ( uint8_t i = 0 ; i < 11 ; i++ ) { @@ -436,3 +440,19 @@ void Sub::default_js_buttons() { get_button(i)->set_default(defaults[i][0], defaults[i][1]); } } + +void Sub::set_neutral_controls() { + int16_t channels[11]; + + for(uint8_t i = 0; i < 7; i++) { + channels[i] = 1500; + } + + for(uint8_t i = 7; i < 11; i++) { + channels[i] = 0xffff; + } + + channels[4] = 0xffff; // Leave mode switch where it was + + failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10); +} diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index 4c0601b574..dfbb067fcf 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) { + if (failsafe.manual_control) { return; } diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 720510fd20..8b58fe0d8a 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -391,7 +391,7 @@ void Sub::update_auto_armed() return; } // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false - if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) { + if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control) { set_auto_armed(false); } }else{ diff --git a/ArduSub/tuning.cpp b/ArduSub/tuning.cpp index cd42ef3e0e..8d39728fbf 100644 --- a/ArduSub/tuning.cpp +++ b/ArduSub/tuning.cpp @@ -12,7 +12,7 @@ void Sub::tuning() { // exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero - if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) { + if ((g.radio_tuning <= 0) || failsafe.manual_control) { return; }