Copter: Implement Stop Mode

ArduCopter/GCS_Mavlink.pde

@@ -63,6 +63,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
     case GUIDED:
     case CIRCLE:
     case POSHOLD:
+    case STOP:
         base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
         // APM does in any mode, as that is defined as "system finds its own goal
@@ -173,6 +174,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
     case LAND:
     case OF_LOITER:
     case POSHOLD:
+    case STOP:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
         break;

ArduCopter/config.h

@@ -562,6 +562,13 @@
  # define POS_XY_P             	1.0f
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Stop mode defaults
+//
+#ifndef STOP_MODE_DECEL_RATE
+ # define STOP_MODE_DECEL_RATE  750 // acceleration rate in cm/s/s in Stop Mode
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Velocity (horizontal) gains
 //

ArduCopter/defines.h

@@ -107,7 +107,8 @@ enum aux_sw_func {
 #define FLIP        14                  // flip the vehicle on the roll axis
 #define AUTOTUNE    15                  // autotune the vehicle's roll and pitch gains
 #define POSHOLD     16                  // position hold with manual override
-#define NUM_MODES   17
+#define STOP        17                  // Full-Stop using inertial/GPS system, no pilot input
+#define NUM_MODES   18  
 
 
 // CH_6 Tuning

ArduCopter/flight_mode.pde

@@ -89,6 +89,10 @@ static bool set_mode(uint8_t mode)
             break;
 #endif
 
+        case STOP:
+            success = stop_init(ignore_checks);
+            break;
+
         default:
             success = false;
             break;
@@ -197,6 +201,10 @@ static void update_flight_mode()
             poshold_run();
             break;
 #endif
+
+        case STOP:
+            stop_run();
+            break;
     }
 }
 
@@ -243,6 +251,7 @@ static bool mode_requires_GPS(uint8_t mode) {
         case CIRCLE:
         case DRIFT:
         case POSHOLD:
+        case STOP:
             return true;
         default:
             return false;