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();