Sub: Manual control failsafe

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

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;
     }
 

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;

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();

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);
     }

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)) {

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)) {

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;

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) {

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

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);
+}

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;
     }
 

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{

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;
     }