AC_WPNav: update target speed immediately

libraries/AC_WPNav/AC_WPNav.cpp

@@ -140,6 +140,22 @@ void AC_WPNav::init_loiter_target()
     _pilot_accel_rgt_cms = 0;
 }
 
+/// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
+void AC_WPNav::set_loiter_velocity(float velocity_cms)
+{
+    // range check velocity and update position controller
+    if (velocity_cms >= WPNAV_LOITER_SPEED_MIN) {
+        _loiter_speed_cms = velocity_cms;
+
+        // initialise pos controller speed
+        _pos_control.set_speed_xy(_loiter_speed_cms);
+
+        // initialise pos controller acceleration
+        _loiter_accel_cms = _loiter_speed_cms/2.0f;
+        _pos_control.set_accel_xy(_loiter_accel_cms);
+    }
+}
+
 /// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
 void AC_WPNav::set_pilot_desired_acceleration(float control_roll, float control_pitch)
 {
@@ -164,8 +180,8 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
     }
 
     // check loiter speed and avoid divide by zero
-    if( _loiter_speed_cms < 100.0f) {
-        _loiter_speed_cms = 100.0f;
+    if( _loiter_speed_cms < WPNAV_LOITER_SPEED_MIN) {
+        _loiter_speed_cms = WPNAV_LOITER_SPEED_MIN;
         _loiter_accel_cms = _loiter_speed_cms/2.0f;
     }
 
@@ -243,6 +259,16 @@ void AC_WPNav::update_loiter()
 /// waypoint navigation
 ///
 
+/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
+void AC_WPNav::set_horizontal_velocity(float velocity_cms)
+{
+    // range check new target speed and update position controller
+    if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) {
+        _wp_speed_cms = velocity_cms;
+        _pos_control.set_speed_xy(_wp_speed_cms);
+    }
+}
+
 /// set_destination - set destination using cm from home
 void AC_WPNav::set_wp_destination(const Vector3f& destination)
 {

libraries/AC_WPNav/AC_WPNav.h

@@ -16,6 +16,7 @@
                                                     // max acceleration = max lean angle * 980 * pi / 180.  i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
 
 #define WPNAV_LOITER_SPEED              500.0f      // maximum default loiter speed in cm/s
+#define WPNAV_LOITER_SPEED_MIN          100.0f      // minimum loiter speed in cm/s
 #define WPNAV_LOITER_ACCEL_MAX          250.0f      // maximum acceleration in loiter mode
 #define WPNAV_LOITER_ACCEL_MIN           25.0f      // minimum acceleration in loiter mode
 #define WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR 200.0f      // maximum speed used to correct position error (i.e. not including feed forward)
@@ -23,6 +24,7 @@
 #define MAX_LEAN_ANGLE                  4500        // default maximum lean angle
 
 #define WPNAV_WP_SPEED                  500.0f      // default horizontal speed betwen waypoints in cm/s
+#define WPNAV_WP_SPEED_MIN              100.0f      // minimum horitzontal speed between waypoints in cm/s
 #define WPNAV_WP_RADIUS                 200.0f      // default waypoint radius in cm
 
 #define WPNAV_WP_SPEED_UP               250.0f      // default maximum climb velocity
@@ -66,7 +68,7 @@ public:
     void init_loiter_target();
 
     /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
-    void set_loiter_velocity(float velocity_cms) { _loiter_speed_cms = velocity_cms; };
+    void set_loiter_velocity(float velocity_cms);
 
     /// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
     void calculate_loiter_leash_length();
@@ -91,16 +93,16 @@ public:
     ///
 
     /// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
-    void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; };
+    void set_horizontal_velocity(float velocity_cms);
 
     /// get_horizontal_velocity - allows main code to retrieve target horizontal velocity for wp navigation
-    float get_horizontal_velocity() { return _wp_speed_cms; };
+    float get_horizontal_velocity() const { return _wp_speed_cms; }
 
     /// get_climb_velocity - returns target climb speed in cm/s during missions
-    float get_climb_velocity() const { return _wp_speed_up_cms; };
+    float get_climb_velocity() const { return _wp_speed_up_cms; }
 
     /// get_descent_velocity - returns target descent speed in cm/s during missions.  Note: always positive
-    float get_descent_velocity() const { return _wp_speed_down_cms; };
+    float get_descent_velocity() const { return _wp_speed_down_cms; }
 
     /// get_wp_radius - access for waypoint radius in cm
     float get_wp_radius() const { return _wp_radius_cm; }