Copter: add brake_timeout_to_loiter_ms

ArduCopter/Copter.h

@@ -375,6 +375,10 @@ private:
     uint16_t loiter_time_max;                // How long we should stay in Loiter Mode for mission scripting (time in seconds)
     uint32_t loiter_time;                    // How long have we been loitering - The start time in millis
 
+    // Brake
+    uint32_t brake_timeout_start;
+    uint32_t brake_timeout_ms;
+
     // Flip
     Vector3f flip_orig_attitude;         // original copter attitude before flip
 
@@ -761,6 +765,7 @@ private:
     void adsb_handle_vehicle_threats(void);
     bool brake_init(bool ignore_checks);
     void brake_run();
+    void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
     bool circle_init(bool ignore_checks);
     void circle_run();
     bool drift_init(bool ignore_checks);

ArduCopter/control_brake.cpp

@@ -25,6 +25,8 @@ bool Copter::brake_init(bool ignore_checks)
         pos_control.set_alt_target(inertial_nav.get_altitude());
         pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
+        brake_timeout_ms = 0;
+
         return true;
     }else{
         return false;
@@ -75,4 +77,17 @@ void Copter::brake_run()
     // update altitude target and call position controller
     pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
     pos_control.update_z_controller();
+
+    if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms)
+    {
+        if(!set_mode(LOITER)) {
+            set_mode(ALT_HOLD);
+        }
+    }
+}
+
+void Copter::brake_timeout_to_loiter_ms(uint32_t timeout_ms)
+{
+    brake_timeout_start = millis();
+    brake_timeout_ms = timeout_ms;
 }