AC_WPNav: handle ekf position reset in Loiter and Brake

libraries/AC_WPNav/AC_WPNav.cpp

@@ -112,6 +112,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosContro
     _loiter_step(0),
     _pilot_accel_fwd_cms(0),
     _pilot_accel_rgt_cms(0),
+    _loiter_ekf_pos_reset_ms(0),
     _wp_last_update(0),
     _wp_step(0),
     _track_length(0.0f),
@@ -181,6 +182,9 @@ void AC_WPNav::init_loiter_target()
     const Vector3f& curr_pos = _inav.get_position();
     const Vector3f& curr_vel = _inav.get_velocity();
 
+    // initialise ekf position reset check
+    init_ekf_position_reset();
+
     // initialise position controller
     _pos_control.init_xy_controller();
 
@@ -323,6 +327,9 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
         if (dt >= 0.2f) {
             dt = 0.0f;
         }
+        // initialise ekf position reset check
+        check_for_ekf_position_reset();
+
         calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
         _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, true);
     }
@@ -334,6 +341,9 @@ void AC_WPNav::init_brake_target(float accel_cmss)
     const Vector3f& curr_vel = _inav.get_velocity();
     Vector3f stopping_point;
 
+    // initialise ekf position reset check
+    init_ekf_position_reset();
+
     // initialise position controller
     _pos_control.init_xy_controller();
 
@@ -1047,3 +1057,22 @@ float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss)
         return target_speed;
     }
 }
+
+/// initialise ekf position reset check
+void AC_WPNav::init_ekf_position_reset()
+{
+    Vector2f pos_shift;
+    _loiter_ekf_pos_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+}
+
+/// check for ekf position reset and adjust loiter or brake target position
+void AC_WPNav::check_for_ekf_position_reset()
+{
+    // check for position shift
+    Vector2f pos_shift;
+    uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
+    if (reset_ms != _loiter_ekf_pos_reset_ms) {
+        _pos_control.shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
+        _loiter_ekf_pos_reset_ms = reset_ms;
+    }
+}

libraries/AC_WPNav/AC_WPNav.h

@@ -257,6 +257,10 @@ protected:
     /// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)
     float get_slow_down_speed(float dist_from_dest_cm, float accel_cmss);
 
+    /// initialise and check for ekf position reset and adjust loiter or brake target position
+    void init_ekf_position_reset();
+    void check_for_ekf_position_reset();
+
     /// spline protected functions
 
     /// update_spline_solution - recalculates hermite_spline_solution grid
@@ -292,6 +296,7 @@ protected:
     int16_t     _pilot_accel_fwd_cms; 	// pilot's desired acceleration forward (body-frame)
     int16_t     _pilot_accel_rgt_cms;   // pilot's desired acceleration right (body-frame)
     Vector2f    _loiter_desired_accel;  // slewed pilot's desired acceleration in lat/lon frame
+    uint32_t    _loiter_ekf_pos_reset_ms;   // system time of last recorded ekf position reset
 
     // waypoint controller internal variables
     uint32_t    _wp_last_update;        // time of last update_wpnav call