AC_PosControl: add xyz velocity controller

Velocity controller interpretsthe velocity requests as
desired velocities (i.e. feed forward).  These are then used to update
the target position and also added to the target velocity.
Also renamed the set_desired_velocity() function to
set_desired_velocity_xy() to make clear only lat and lon axis are
updated.

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -37,6 +37,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
     _dt(POSCONTROL_DT_10HZ),
     _last_update_xy_ms(0),
     _last_update_z_ms(0),
+    _last_update_vel_xyz_ms(0),
     _speed_down_cms(POSCONTROL_SPEED_DOWN),
     _speed_up_cms(POSCONTROL_SPEED_UP),
     _speed_cms(POSCONTROL_SPEED),
@@ -48,7 +49,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
     _alt_max(0),
     _distance_to_target(0),
     _xy_step(0),
-    _dt_xy(0)
+    _dt_xy(0),
+    _vel_xyz_step(0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -580,6 +582,81 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
     }
 }
 
+/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
+void AC_PosControl::init_vel_controller_xyz()
+{
+    // force the xy velocity controller to run immediately
+    _vel_xyz_step = 3;
+
+    // set roll, pitch lean angle targets to current attitude
+    _roll_target = _ahrs.roll_sensor;
+    _pitch_target = _ahrs.pitch_sensor;
+
+    // reset last velocity if this controller has just been engaged or dt is zero
+    lean_angles_to_accel(_accel_target.x, _accel_target.y);
+    _pid_rate_lat.set_integrator(_accel_target.x);
+    _pid_rate_lon.set_integrator(_accel_target.y);
+
+    // flag reset required in rate to accel step
+    _flags.reset_desired_vel_to_pos = true;
+    _flags.reset_rate_to_accel_xy = true;
+
+    // set target position in xy axis
+    const Vector3f& curr_pos = _inav.get_position();
+    set_xy_target(curr_pos.x, curr_pos.y);
+
+    // move current vehicle velocity into feed forward velocity
+    const Vector3f& curr_vel = _inav.get_velocity();
+    set_desired_velocity_xy(curr_vel.x, curr_vel.y);
+
+    // record update time
+    _last_update_vel_xyz_ms = hal.scheduler->millis();
+}
+
+/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
+///     velocity targets should we set using set_desired_velocity_xyz() method
+///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
+///     throttle targets will be sent directly to the motors
+void AC_PosControl::update_vel_controller_xyz()
+{
+    // capture time since last iteration
+    uint32_t now = hal.scheduler->millis();
+    float dt_xy = (now - _last_update_vel_xyz_ms) / 1000.0f;
+
+    // check if xy leash needs to be recalculated
+    calc_leash_length_xy();
+
+    // we will run the horizontal component every 4th iteration (i.e. 50hz on Pixhawk, 20hz on APM)
+    if (dt_xy >= POSCONTROL_VEL_UPDATE_TIME) {
+
+        // record update time
+        _last_update_vel_xyz_ms = now;
+
+        // sanity check dt
+        if (dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
+            dt_xy = 0.0f;
+        }
+
+        // apply desired velocity request to position target
+        desired_vel_to_pos(dt_xy);
+
+        // run position controller's position error to desired velocity step
+        pos_to_rate_xy(true, dt_xy);
+
+        // run velocity to acceleration step
+        rate_to_accel_xy(dt_xy);
+
+        // run acceleration to lean angle step
+        accel_to_lean_angles();
+    }
+
+    // update altitude target
+    set_alt_target_from_climb_rate(_vel_desired.z, _dt);
+
+    // run z-axis position controller
+    update_z_controller();
+}
+
 ///
 /// private methods
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -38,6 +38,8 @@
 
 #define POSCONTROL_ACCEL_Z_DTERM_FILTER         20      // Z axis accel controller's D term filter (in hz)
 
+#define POSCONTROL_VEL_UPDATE_TIME              0.020f  // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
+
 class AC_PosControl
 {
 public:
@@ -169,12 +171,16 @@ public:
     void set_xy_target(float x, float y);
 
     /// get_desired_velocity - returns xy desired velocity (i.e. feed forward) in cm/s in lat and lon direction
-    const Vector2f& get_desired_velocity() { return _vel_desired; }
+    const Vector3f& get_desired_velocity() { return _vel_desired; }
 
-    /// set_desired_velocity - sets desired velocity in cm/s in lat and lon directions
+    /// set_desired_velocity_xy - sets desired velocity in cm/s in lat and lon directions
     ///     when update_xy_controller is next called the position target is moved based on the desired velocity and
     ///     the desired velocities are fed forward into the rate_to_accel step
-    void set_desired_velocity(float vel_lat_cms, float vel_lon_cms) {_vel_desired.x = vel_lat_cms; _vel_desired.y = vel_lon_cms; }
+    void set_desired_velocity_xy(float vel_lat_cms, float vel_lon_cms) {_vel_desired.x = vel_lat_cms; _vel_desired.y = vel_lon_cms; }
+
+    /// set_desired_velocity - sets desired velocity in cm/s in all 3 axis
+    ///     when update_vel_controller_xyz is next called the position target is moved based on the desired velocity
+    void set_desired_velocity(const Vector3f &des_vel) { _vel_desired = des_vel; freeze_ff_xy(); }
 
     /// trigger_xy - used to notify the position controller than an update has been made to the position or desired velocity so that the position controller will run as soon as possible after the update
     void trigger_xy() { _flags.force_recalc_xy = true; }
@@ -205,6 +211,20 @@ public:
     /// get_distance_to_target - get horizontal distance to position target in cm (used for reporting)
     float get_distance_to_target() const;
 
+    /// xyz velocity controller
+
+    /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
+    void init_vel_controller_xyz();
+
+    /// set_vel_target - sets target velocity in cm/s in north, east and up directions
+    void set_vel_target(const Vector3f& vel_target);
+
+    /// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
+    ///     velocity targets should we set using set_desired_velocity_xyz() method
+    ///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
+    ///     throttle targets will be sent directly to the motors
+    void update_vel_controller_xyz();
+
     /// get desired roll, pitch which should be fed into stabilize controllers
     float get_roll() const { return _roll_target; }
     float get_pitch() const { return _pitch_target; }
@@ -316,6 +336,7 @@ private:
     float       _dt;                    // time difference (in seconds) between calls from the main program
     uint32_t    _last_update_xy_ms;     // system time of last update_xy_controller call
     uint32_t    _last_update_z_ms;      // system time of last update_z_controller call
+    uint32_t    _last_update_vel_xyz_ms;// system time of last update_vel_controller_xyz call
     float       _speed_down_cms;        // max descent rate in cm/s
     float       _speed_up_cms;          // max climb rate in cm/s
     float       _speed_cms;             // max horizontal speed in cm/s
@@ -332,7 +353,7 @@ private:
     // position controller internal variables
     Vector3f    _pos_target;            // target location in cm from home
     Vector3f    _pos_error;             // error between desired and actual position in cm
-    Vector2f    _vel_desired;           // desired velocity in cm/s in lat and lon directions (provided by external callers of move_target_at_rate() method)
+    Vector3f    _vel_desired;           // desired velocity in cm/s
     Vector3f    _vel_target;            // velocity target in cm/s calculated by pos_to_rate step
     Vector3f    _vel_error;             // error between desired and actual acceleration in cm/s
     Vector3f    _vel_last;              // previous iterations velocity in cm/s
@@ -343,5 +364,8 @@ private:
     float       _distance_to_target;    // distance to position target - for reporting only
     uint8_t     _xy_step;               // used to decide which portion of horizontal position controller to run during this iteration
     float       _dt_xy;                 // time difference in seconds between horizontal position updates
+
+    // velocity controller internal variables
+    uint8_t     _vel_xyz_step;          // used to decide which portion of velocity controller to run during this iteration
 };
 #endif	// AC_POSCONTROL_H