PosControlFixes

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -417,7 +417,7 @@ float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_
     if (is_zero(pos_offset_z_buffer)) {
         return 1.0;
     }
-    float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z - _pos_offset.z + pos_offset_z);
+    float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z + (pos_offset_z - _pos_terrain));
     return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0);
 }
 
@@ -540,7 +540,7 @@ void AC_PosControl::init_xy_controller()
     init_ekf_xy_reset();
 
     // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
-    init_offsets_xy_cm();
+    init_offsets_xy();
 
     // initialise z_controller time out
     _last_update_xy_ticks = AP::scheduler().ticks32();
@@ -820,10 +820,10 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
 void AC_PosControl::init_z_controller()
 {
     _pos_target.z = _inav.get_position_z_up_cm();
-    _pos_desired.z = _pos_target.z - (_pos_offset.z - _pos_terrain);
+    _pos_desired.z = _pos_target.z - _pos_offset.z;
 
     _vel_target.z = _inav.get_velocity_z_up_cms();
-    _vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
+    _vel_desired.z = _vel_target.z - _vel_offset.z;
 
     // Reset I term of velocity PID
     _pid_vel_z.reset_filter();
@@ -834,7 +834,8 @@ void AC_PosControl::init_z_controller()
     _pid_accel_z.reset_filter();
 
     // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
-    init_offsets_z_cm();
+    init_terrain();
+    init_offsets_z();
 
     // Set accel PID I term based on the current throttle
     // Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss
@@ -1113,6 +1114,21 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c
 
 /// Terrain
 
+/// set the terrain position, velocity and acceleration in cm, cms and cm/s/s from EKF origin in NE frame
+/// this is used to initiate the offsets when initialise the position controller or do an offset reset
+void AC_PosControl::init_terrain()
+{
+    // set terrain position and target to zero
+    _pos_terrain_target = 0.0;
+    _pos_terrain = 0.0;
+
+    // set velocity offset to zero
+    _vel_terrain = 0.0;
+
+    // set acceleration offset to zero
+    _accel_terrain = 0.0;
+}
+
 // remove offsets from the position velocity and acceleration targets
 void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm)
 {
@@ -1126,7 +1142,7 @@ void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm)
 
 /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
 /// this is used to initiate the offsets when initialise the position controller or do an offset reset
-void AC_PosControl::init_offsets_xy_cm()
+void AC_PosControl::init_offsets_xy()
 {
     // check for offset target timeout
     uint32_t now_ms = AP_HAL::millis();
@@ -1148,7 +1164,7 @@ void AC_PosControl::init_offsets_xy_cm()
 
 /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
 /// this is used to initiate the offsets when initialise the position controller or do an offset reset
-void AC_PosControl::init_offsets_z_cm()
+void AC_PosControl::init_offsets_z()
 {
     // check for offset target timeout
     uint32_t now_ms = AP_HAL::millis();

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -474,20 +474,25 @@ class AC_PosControl
 
     /// Terrain Following
 
+    /// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
+    /// this is used to initiate the offsets when initialise the position controller or do an offset reset
+    /// note that this sets the actual offsets, not the offset targets
+    void init_terrain();
+
     /// update_terrain - updates the terrain position, velocity and acceleration estimation
     /// this moves the estimated terrain position _pos_terrain towards the target _pos_terrain_target
     void update_terrain();
 
 
     /// Offsets
 
-    /// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
+    /// init_offsets - set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
     /// this is used to initiate the offsets when initialise the position controller or do an offset reset
     /// note that this sets the actual offsets, not the offset targets
-    void init_offsets_xy_cm();
-    void init_offsets_z_cm();
+    void init_offsets_xy();
+    void init_offsets_z();
 
-    /// update the position and velocity offsets
+    /// update_offsets - update the position and velocity offsets
     /// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target)
     void update_offsets_xy();
     void update_offsets_z();