AC_PosControl: add ekf position reset handling

Previously this was in AC_WPNav and used only for loiter but it should work for any flight modes that use horizontal or vertical position control

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -230,6 +230,17 @@ void AC_PosControl::add_takeoff_climb_rate(float climb_rate_cms, float dt)
     _pos_target.z += climb_rate_cms * dt;
 }
 
+/// shift altitude target (positive means move altitude up)
+void AC_PosControl::shift_alt_target(float z_cm)
+{
+    _pos_target.z += z_cm;
+
+    // freeze feedforward to avoid jump
+    if (!is_zero(z_cm)) {
+        freeze_ff_z();
+    }
+}
+
 /// relax_alt_hold_controllers - set all desired and targets to measured
 void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
 {
@@ -306,6 +317,9 @@ void AC_PosControl::init_takeoff()
 
     // shift difference between last motor out and hover throttle into accelerometer I
     _pid_accel_z.set_integrator((_motors.get_throttle()-_motors.get_throttle_hover())*1000.0f);
+
+    // initialise ekf reset handler
+    init_ekf_z_reset();
 }
 
 // is_active_z - returns true if the z-axis position controller has been run very recently
@@ -325,6 +339,9 @@ void AC_PosControl::update_z_controller()
     }
     _last_update_z_ms = now;
 
+    // check for ekf altitude reset
+    check_for_ekf_z_reset();
+
     // check if leash lengths need to be recalculated
     calc_leash_length_z();
 
@@ -642,6 +659,9 @@ void AC_PosControl::init_xy_controller(bool reset_I)
     _flags.reset_desired_vel_to_pos = true;
     _flags.reset_rate_to_accel_xy = true;
     _flags.reset_accel_to_lean_xy = true;
+
+    // initialise ekf xy reset handler
+    init_ekf_xy_reset();
 }
 
 /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
@@ -657,6 +677,9 @@ void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler
         dt = 0.0f;
     }
 
+    // check for ekf xy position reset
+    check_for_ekf_xy_reset();
+
     // check if xy leash needs to be recalculated
     calc_leash_length_xy();
 
@@ -703,6 +726,10 @@ void AC_PosControl::init_vel_controller_xyz()
     // move current vehicle velocity into feed forward velocity
     const Vector3f& curr_vel = _inav.get_velocity();
     set_desired_velocity(curr_vel);
+
+    // initialise ekf reset handlers
+    init_ekf_xy_reset();
+    init_ekf_z_reset();
 }
 
 /// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
@@ -722,6 +749,9 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
             dt = 0.0f;
         }
 
+        // check for ekf xy position reset
+        check_for_ekf_xy_reset();
+
         // check if xy leash needs to be recalculated
         calc_leash_length_xy();
 
@@ -1020,3 +1050,41 @@ float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float k
 
     return leash_length;
 }
+
+/// initialise ekf xy position reset check
+void AC_PosControl::init_ekf_xy_reset()
+{
+    Vector2f pos_shift;
+    _ekf_xy_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+}
+
+/// check for ekf position reset and adjust loiter or brake target position
+void AC_PosControl::check_for_ekf_xy_reset()
+{
+    // check for position shift
+    Vector2f pos_shift;
+    uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+    if (reset_ms != _ekf_xy_reset_ms) {
+        shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
+        _ekf_xy_reset_ms = reset_ms;
+    }
+}
+
+/// initialise ekf z axis reset check
+void AC_PosControl::init_ekf_z_reset()
+{
+    float alt_shift;
+    _ekf_z_reset_ms = _ahrs.getLastPosDownReset(alt_shift);
+}
+
+/// check for ekf position reset and adjust loiter or brake target position
+void AC_PosControl::check_for_ekf_z_reset()
+{
+    // check for position shift
+    float alt_shift;
+    uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
+    if (reset_ms != _ekf_z_reset_ms) {
+        shift_alt_target(-alt_shift * 100.0f);
+        _ekf_z_reset_ms = reset_ms;
+    }
+}

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -137,6 +137,9 @@ public:
     /// set_alt_target_to_current_alt - set altitude target to current altitude
     void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }
 
+    /// shift altitude target (positive means move altitude up)
+    void shift_alt_target(float z_cm);
+
     /// relax_alt_hold_controllers - set all desired and targets to measured
     void relax_alt_hold_controllers(float throttle_setting);
 
@@ -358,6 +361,12 @@ private:
     /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
     float calc_leash_length(float speed_cms, float accel_cms, float kP) const;
 
+    /// initialise and check for ekf position resets
+    void init_ekf_xy_reset();
+    void check_for_ekf_xy_reset();
+    void init_ekf_z_reset();
+    void check_for_ekf_z_reset();
+
     // references to inertial nav and ahrs libraries
     const AP_AHRS&              _ahrs;
     const AP_InertialNav&       _inav;
@@ -411,4 +420,8 @@ private:
 
     Vector2f    _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s
     LowPassFilterVector2f _accel_target_filter; // acceleration target filter
+
+    // 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
 };