EKF rest fix

lthall · Sep 20, 2024 · 7458660 · 7458660
1 parent 0cb616a
commit 7458660
Showing 1 changed file with 15 additions and 6 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1329,9 +1329,9 @@ void AC_PosControl::write_log()
                    _accel_desired.y, _accel_target.y, accel_y);
 
         // log offsets if they have ever been used
-        if (!_pos_offset_target.xy().is_zero()) {
+//        if (!_pos_offset_target.xy().is_zero()) {
             Write_PSCO(_pos_offset_target.xy(), _pos_offset.xy(), _vel_offset_target.xy(), _vel_offset.xy(), _accel_offset_target.xy(), _accel_offset.xy());
-        }
+//        }
     }
 
     if (is_active_z()) {
@@ -1476,8 +1476,13 @@ void AC_PosControl::handle_ekf_xy_reset()
     uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
     if (reset_ms != _ekf_xy_reset_ms) {
 
-        _pos_offset.xy() += _pos_target.xy() - (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
-        _vel_offset.xy() += _vel_target.xy() - (_inav.get_velocity_xy_cms() + _pid_vel_xy.get_error());
+        //_pos_offset.xy() += _pos_target.xy() - (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
+        //_vel_offset.xy() += _vel_target.xy() - (_inav.get_velocity_xy_cms() + _pid_vel_xy.get_error());
+
+        _pos_target.xy() = (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
+        _pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
+        _vel_target.xy() = _inav.get_velocity_xy_cms() + _pid_vel_xy.get_error();
+        _vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
 
         _ekf_xy_reset_ms = reset_ms;
     }
@@ -1498,8 +1503,12 @@ void AC_PosControl::handle_ekf_z_reset()
     uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
     if (reset_ms != 0 && reset_ms != _ekf_z_reset_ms) {
 
-        _pos_offset.z += _pos_target.z - (_inav.get_position_z_up_cm() + _p_pos_z.get_error());
-        _vel_offset.z += _vel_target.z - (_inav.get_velocity_z_up_cms() + _pid_vel_z.get_error());
+        //_pos_offset.z += _pos_target.z - (_inav.get_position_z_up_cm() + _p_pos_z.get_error());
+        //_vel_offset.z += _vel_target.z - (_inav.get_velocity_z_up_cms() + _pid_vel_z.get_error());
+        _pos_target.z = _inav.get_position_z_up_cm() + _p_pos_z.get_error();
+        _pos_desired.z = _pos_target.z - (_pos_offset.z - _pos_terrain);
+        _vel_target.z = _inav.get_velocity_z_up_cms() + _pid_vel_z.get_error();
+        _vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
 
         _ekf_z_reset_ms = reset_ms;
     }