AC_AttitudeControl: Add terain following to guided

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -334,9 +334,12 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
 ///     The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
 ///     The time constant also defines the time taken to achieve the maximum acceleration.
 ///     The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
-void AC_PosControl::input_pos_xyz(const Vector3p& pos)
+void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
 {
-    Vector3f dest_vector = (pos - _pos_target).tofloat();
+    // remove terrain offsets for flat earth assumption
+    _pos_target.z -= _pos_offset_z;
+    _vel_desired.z -= _vel_offset_z;
+    _accel_desired.z -= _accel_offset_z;
 
     // calculated increased maximum acceleration if over speed
     float accel_z_cmss = _accel_max_z_cmss;
@@ -349,31 +352,41 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos)
 
     update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy());
 
-    // adjust desired alt if motors have not hit their limits
+    // adjust desired altitude if motors have not hit their limits
     update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z);
 
-    float vel_max_xy_cms = _vel_max_xy_cms;
+    // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
+    float vel_max_xy_cms = 0.0f;
     float vel_max_z_cms = 0.0f;
+    Vector3f dest_vector = (pos - _pos_target).tofloat();
     if (is_positive(dest_vector.length_squared()) ) {
         dest_vector.normalize();
         float dest_vector_xy_length = dest_vector.xy().length();
 
         float vel_max_cms = kinematic_limit(dest_vector, _vel_max_xy_cms, _vel_max_up_cms, _vel_max_down_cms);
         vel_max_xy_cms = vel_max_cms * dest_vector_xy_length;
-        vel_max_z_cms = vel_max_cms * dest_vector.z;
+        vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z);
     }
 
-
     Vector2f vel;
     Vector2f accel;
     shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
                            vel_max_xy_cms, _vel_max_xy_cms, _accel_max_xy_cmss, _tc_xy_s, _dt);
+
     float posz = pos.z;
     shape_pos_vel_accel(posz, 0, 0,
                         _pos_target.z, _vel_desired.z, _accel_desired.z,
                         vel_max_z_cms, _vel_max_down_cms, _vel_max_up_cms,
                         -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
                         _tc_z_s, _dt);
+
+    // update the vertical position, velocity and acceleration offsets
+    update_pos_offset_z(pos_offset_z);
+
+    // add terrain offsets
+    _pos_target.z += _pos_offset_z;
+    _vel_desired.z += _vel_offset_z;
+    _accel_desired.z += _accel_offset_z;
 }
 
 ///
@@ -743,6 +756,11 @@ void AC_PosControl::init_z()
     _accel_target.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f;
     _pid_accel_z.reset_filter();
 
+    // initialise vertical offsets
+    _pos_offset_z = 0.0;
+    _vel_offset_z = 0.0;
+    _accel_offset_z = 0.0;
+
     // initialise ekf z reset handler
     init_ekf_z_reset();
 
@@ -806,7 +824,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac
         accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
     }
 
-    // adjust desired alt if motors have not hit their limits
+    // adjust desired altitude if motors have not hit their limits
     update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z);
 
     shape_pos_vel_accel(pos, vel, accel,
@@ -829,6 +847,21 @@ void AC_PosControl::set_alt_target_with_slew(const float& pos)
     input_pos_vel_accel_z(posf, zero, 0);
 }
 
+/// update_pos_offset_z - updates the vertical offsets used by terrain following
+void AC_PosControl::update_pos_offset_z(float pos_offset_z)
+{
+
+    postype_t posp_z = _pos_offset_z;
+    update_pos_vel_accel(posp_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f));
+    _pos_offset_z = posp_z;
+
+    // input shape the terrain offset
+    shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,
+        _pos_offset_z, _vel_offset_z, _accel_offset_z,
+        0.0f, get_max_speed_down_cms(), get_max_speed_up_cms(),
+        -get_max_accel_z_cmss(), get_max_accel_z_cmss(), _tc_z_s, _dt);
+}
+
 // is_active_z - returns true if the z position controller has been run in the previous 5 loop times
 bool AC_PosControl::is_active_z() const
 {
@@ -857,7 +890,9 @@ void AC_PosControl::update_z_controller()
     const float curr_alt = _inav.get_position().z;
     // calculate the target velocity correction
     float pos_target_zf = _pos_target.z;
+
     _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up);
+
     _pos_target.z = pos_target_zf;
 
     // add feed forward component
@@ -868,6 +903,7 @@ void AC_PosControl::update_z_controller()
     const Vector3f& curr_vel = _inav.get_velocity();
     _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
 
+    // add feed forward component
     _accel_target.z += _accel_desired.z;
 
     // Acceleration Controller

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -57,7 +57,7 @@ public:
     /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
     ///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
     ///     The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
-    void input_pos_xyz(const Vector3p& pos);
+    void input_pos_xyz(const Vector3p& pos, float pos_offset_z);
 
     ///
     /// Lateral position controller
@@ -184,6 +184,9 @@ public:
     ///     using the default position control kinimatic path.
     void set_alt_target_with_slew(const float& pos);
 
+    /// update_pos_offset_z - updates the vertical offsets used by terrain following
+    void update_pos_offset_z(float pos_offset);
+
     // is_active_z - returns true if the z position controller has been run in the previous 5 loop times
     bool is_active_z() const;
 
@@ -215,7 +218,7 @@ public:
     /// set_pos_target_z_cm - set altitude target in cm above home
     void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; }
 
-    /// get_pos_target_z_cm - get desired altitude (in cm above home)
+    /// get_pos_target_z_cm - get target altitude (in cm above home)
     float get_pos_target_z_cm() const { return _pos_target.z; }
 
     /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
@@ -264,6 +267,12 @@ public:
     const Vector3f& get_accel_target_cmss() const { return _accel_target; }
 
 
+    /// Offset
+
+    /// set_pos_offset_z_cm - set altitude offset in cm above home
+    void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; }
+
+
     /// Outputs
 
     /// get desired roll and pitch to be passed to the attitude controller
@@ -413,10 +422,13 @@ protected:
     Vector3f    _accel_target;          // acceleration target in NEU cm/s/s
     Vector3f    _limit_vector;          // the direction that the position controller is limited, zero when not limited
     Vector2f    _vehicle_horiz_vel;     // velocity to use if _flags.vehicle_horiz_vel_override is set
+    float       _pos_offset_z;          // vertical position offset in NEU cm from home
+    float       _vel_offset_z;          // vertical velocity offset in NEU cm/s calculated by pos_to_rate step
+    float       _accel_offset_z;        // vertical acceleration offset in NEU cm/s/s
 
     // ekf reset handling
-    uint32_t    _ekf_xy_reset_ms;      // system time of last recorded ekf xy position reset
-    uint32_t    _ekf_z_reset_ms;       // system time of last recorded ekf altitude reset
+    uint32_t    _ekf_xy_reset_ms;       // system time of last recorded ekf xy position reset
+    uint32_t    _ekf_z_reset_ms;        // system time of last recorded ekf altitude reset
 
     // high vibration handling
     bool        _vibe_comp_enabled;     // true when high vibration compensation is on