Copter: brake moved in from AC_WPNav

ArduCopter/mode.h

@@ -532,6 +532,8 @@ protected:
 
 private:
 
+    void init_target();
+
     uint32_t _timeout_start;
     uint32_t _timeout_ms;
 

ArduCopter/mode_brake.cpp

@@ -10,7 +10,7 @@
 bool ModeBrake::init(bool ignore_checks)
 {
     // set target to current position
-    wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
+    init_target();
 
     // initialize vertical speed and acceleration
     pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
@@ -34,7 +34,7 @@ void ModeBrake::run()
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_spool_down();
-        wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
+        init_target();
         return;
     }
 
@@ -46,8 +46,9 @@ void ModeBrake::run()
         loiter_nav->soften_for_landing();
     }
 
-    // run brake controller
-    wp_nav->update_brake();
+    // use position controller to stop
+    pos_control->set_desired_velocity_xy(0.0f, 0.0f);
+    pos_control->update_xy_controller();
 
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);
@@ -74,4 +75,22 @@ void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)
     _timeout_ms = timeout_ms;
 }
 
+void ModeBrake::init_target()
+{
+    // initialise position controller
+    pos_control->set_desired_velocity_xy(0.0f,0.0f);
+    pos_control->set_desired_accel_xy(0.0f,0.0f);
+    pos_control->init_xy_controller();
+
+    // initialise pos controller speed and acceleration
+    pos_control->set_max_speed_xy(inertial_nav.get_velocity().length());
+    pos_control->set_max_accel_xy(BRAKE_MODE_DECEL_RATE);
+    pos_control->calc_leash_length_xy();
+
+    // set target position
+    Vector3f stopping_point;
+    pos_control->get_stopping_point_xy(stopping_point);
+    pos_control->set_xy_target(stopping_point.x, stopping_point.y);
+}
+
 #endif