Fixed the pattern I think

ArduCopter/mode.cpp

@@ -1084,11 +1084,10 @@ uint16_t Mode::get_pilot_speed_dn()
 }
 
 // Return stopping point as a location with above origin alt frame
-// subtract_pos_offsets should be true if the stopping point will be used as a position controller target (because the pos controller will add back the offsets)
-Location Mode::get_stopping_point(bool subtract_pos_offsets) const
+Location Mode::get_stopping_point() const
 {
     Vector3p stopping_point_NEU;
-    copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy(), subtract_pos_offsets);
+    copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
     copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
     return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
 }

ArduCopter/mode.h

@@ -202,8 +202,7 @@ class Mode {
     void zero_throttle_and_hold_attitude();
 
     // Return stopping point as a location with above origin alt frame
-    // subtract_pos_offsets should be true if the stopping point will be used as a position controller target (because the pos controller will add back the offsets)
-    Location get_stopping_point(bool subtract_pos_offsets = false) const;
+    Location get_stopping_point() const;
 
     // functions to control normal landing.  pause_descent is true if vehicle should not descend
     void land_run_horizontal_control();

ArduCopter/mode_auto.cpp

@@ -78,10 +78,6 @@ void ModeAuto::exit()
 #endif  // HAL_MOUNT_ENABLED
 
     auto_RTL = false;
-
-    // clear position, velocity and acceleration offsets
-    const Vector2f zero_vec;
-    pos_control->set_posvelaccel_offset_target_xy_cm(Vector2p{0, 0}, zero_vec, zero_vec);
 }
 
 // auto_run - runs the auto controller
@@ -345,7 +341,7 @@ bool ModeAuto::loiter_start()
 
     // calculate stopping point
     Vector3f stopping_point;
-    wp_nav->get_wp_stopping_point(stopping_point, true);
+    wp_nav->get_wp_stopping_point(stopping_point);
 
     // initialise waypoint controller target to stopping point
     wp_nav->set_wp_destination(stopping_point);

ArduCopter/mode_rtl.cpp

@@ -376,8 +376,8 @@ void ModeRTL::land_run(bool disarm_on_land)
 
 void ModeRTL::build_path()
 {
-    // origin point is our stopping point minus any position offsets
-    rtl_path.origin_point = get_stopping_point(true);
+    // origin point is our stopping point
+    rtl_path.origin_point = get_stopping_point();
     rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
 
     // compute return target

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -546,6 +546,10 @@ void AC_PosControl::init_xy_controller()
     // initialise ekf xy reset handler
     init_ekf_xy_reset();
 
+    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
+    update_xy_offsets();
+    set_posvelaccel_offsets_xy_cm(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target);
+
     // initialise z_controller time out
     _last_update_xy_ticks = AP::scheduler().ticks32();
 }
@@ -561,9 +565,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
     handle_ekf_xy_reset();
 
     // remove offsets
-    _pos_target.xy() -= _pos_offset.xy();
-    _vel_desired.xy() -= _vel_offset.xy();
-    _accel_desired.xy() -= _accel_offset.xy();
+    remove_offsets_xy();
 
     update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
     shape_accel_xy(accel, _accel_desired, _jerk_max_xy_cmsss, _dt);
@@ -572,9 +574,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
     update_xy_offsets();
 
     // add back offsets
-    _pos_target.xy() += _pos_offset.xy();
-    _vel_desired.xy() += _vel_offset.xy();
-    _accel_desired.xy() += _accel_offset.xy();
+    add_offsets_xy();
 }
 
 /// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
@@ -585,9 +585,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
 void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output)
 {
     // remove offsets including terrain offset for flat earth assumption
-    _pos_target.xy() -= _pos_offset.xy();
-    _vel_desired.xy() -= _vel_offset.xy();
-    _accel_desired.xy() -= _accel_offset.xy();
+    remove_offsets_xy();
 
     update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
 
@@ -600,9 +598,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo
     update_xy_offsets();
 
     // add back offsets including terrain offset
-    _pos_target.xy() += _pos_offset.xy();
-    _vel_desired.xy() += _vel_offset.xy();
-    _accel_desired.xy() += _accel_offset.xy();
+    add_offsets_xy();
 }
 
 /// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
@@ -613,9 +609,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo
 void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output)
 {
     // remove offsets including terrain offset for flat earth assumption
-    _pos_target.xy() -= _pos_offset.xy();
-    _vel_desired.xy() -= _vel_offset.xy();
-    _accel_desired.xy() -= _accel_offset.xy();
+    remove_offsets_xy();
 
     update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
 
@@ -628,9 +622,7 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const V
     update_xy_offsets();
 
     // add back offsets including terrain offset
-    _pos_target.xy() += _pos_offset.xy();
-    _vel_desired.xy() += _vel_offset.xy();
-    _accel_desired.xy() += _accel_offset.xy();
+    add_offsets_xy();
 }
 
 /// update the horizontal position and velocity offsets
@@ -1164,6 +1156,32 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c
     };
 }
 
+/// 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::set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_xy_cm, const Vector2f& vel_offset_xy_cms, const Vector2f& accel_offset_xy_cmss)
+{
+    // set position offset to target
+    _pos_offset.xy() = pos_offset_xy_cm;
+
+    // set velocity offset to target
+    _vel_offset.xy() = vel_offset_xy_cms;
+
+    // set acceleration offset to target
+    _accel_offset.xy() = accel_offset_xy_cmss;
+}
+
+void AC_PosControl::remove_offsets_xy()
+{
+    // remove offsets
+    remove_offsets_xy();
+}
+
+void AC_PosControl::add_offsets_xy()
+{
+    // add back offsets
+    add_offsets_xy();
+}
+
 /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
 /// these must be set every 3 seconds (or less) or they will timeout and return to zero
 void AC_PosControl::set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss)
@@ -1205,14 +1223,14 @@ Vector3f AC_PosControl::get_thrust_vector() const
 
 /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
 ///    function does not change the z axis
-/// subtract_pos_offsets should be true if the stopping point will be used as a position controller target (because the pos controller will add back the offsets)
-void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point, bool subtract_pos_offsets) const
+void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
 {
-    //stopping_point = _inav.get_position_xy_cm().topostype() - _pos_offset.xy();
+    // todo: we should use the current target position and velocity if we are currently running the position controller
     stopping_point = _inav.get_position_xy_cm().topostype();
     float kP = _p_pos_xy.kP();
 
     Vector2f curr_vel = _inav.get_velocity_xy_cms();
+    curr_vel -= _vel_offset.xy();
 
     // calculate current velocity
     float vel_total = curr_vel.length();
@@ -1229,11 +1247,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point, bool subt
     // convert the stopping distance into a stopping point using velocity vector
     const float t = stopping_dist / vel_total;
     stopping_point += (curr_vel * t).topostype();
-
-    // subtract position offsets if required
-    if (subtract_pos_offsets) {
-        stopping_point -= _pos_offset.xy();
-    }
+    stopping_point -= _pos_offset.xy();
 }
 
 /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -258,7 +258,6 @@ class AC_PosControl
     void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; }
 
     /// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin
-    /// position offsets are not included in the returned value
     const Vector3p& get_pos_target_cm() const { return _pos_target; }
 
     /// set_pos_target_z_cm - set altitude target in cm above the EKF origin
@@ -268,8 +267,7 @@ class AC_PosControl
     float get_pos_target_z_cm() const { return _pos_target.z; }
 
     /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
-    /// subtract_pos_offsets should be true if the stopping point will be used as a position controller target (because the pos controller will add back the offsets)
-    void get_stopping_point_xy_cm(Vector2p &stopping_point, bool subtract_pos_offsets = false) const;
+    void get_stopping_point_xy_cm(Vector2p &stopping_point) const;
 
     /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
     void get_stopping_point_z_cm(postype_t &stopping_point) const;
@@ -316,6 +314,12 @@ class AC_PosControl
 
     /// Offset
 
+    /// 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 set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss);
+    void remove_offsets_xy();
+    void add_offsets_xy();
+
     /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
     /// these must be set every 3 seconds (or less) or they will timeout and return to zero
     void set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss);
@@ -325,6 +329,7 @@ class AC_PosControl
     void set_velaccel_offset_target_xy_cms(const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss);
 
     /// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
+    const Vector3p& get_pos_offset_target_cm() const { return _pos_offset_target; }
     const Vector3p& get_pos_offset_cm() const { return _pos_offset; }
     const Vector3f& get_vel_offset_cms() const { return _vel_offset; }
     const Vector3f& get_accel_offset_cmss() const { return _accel_offset; }

libraries/AC_WPNav/AC_Circle.cpp

@@ -241,7 +241,7 @@ void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) co
 {
     // get current position
     Vector3p stopping_point;
-    _pos_control.get_stopping_point_xy_cm(stopping_point.xy(), true);
+    _pos_control.get_stopping_point_xy_cm(stopping_point.xy());
     _pos_control.get_stopping_point_z_cm(stopping_point.z);
 
     // calc vector from stopping point to circle center

libraries/AC_WPNav/AC_Loiter.cpp

@@ -189,7 +189,9 @@ float AC_Loiter::get_angle_max_cd() const
 /// run the loiter controller
 void AC_Loiter::update(bool avoidance_on)
 {
+    _pos_control.remove_offsets_xy();
     calc_desired_velocity(avoidance_on);
+    _pos_control.add_offsets_xy();
     _pos_control.update_xy_controller();
 }
 

libraries/AC_WPNav/AC_WPNav.cpp

@@ -151,7 +151,7 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
 
 /// wp_and_spline_init - initialise straight line and spline waypoint controllers
 ///     speed_cms should be a positive value or left at zero to use the default speed
-///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero.  pos offsets (if any) should be included
+///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
 ///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
 void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
 {    
@@ -194,7 +194,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
     if (stopping_point.is_zero()) {
         get_wp_stopping_point(stopping_point);
     }
-    _origin = _destination = stopping_point - _pos_control.get_pos_offset_cm().tofloat();
+    _origin = _destination = stopping_point;
     _terrain_alt = false;
     _this_leg_is_spline = false;
 
@@ -439,20 +439,18 @@ void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
 }
 
 /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
-/// subtract_pos_offsets should be true if the stopping point will be used as a position target (because the pos controller will add back the offsets)
-void AC_WPNav::get_wp_stopping_point_xy(Vector2f& stopping_point, bool subtract_pos_offsets) const
+void AC_WPNav::get_wp_stopping_point_xy(Vector2f& stopping_point) const
 {
     Vector2p stop;
-    _pos_control.get_stopping_point_xy_cm(stop, subtract_pos_offsets);
+    _pos_control.get_stopping_point_xy_cm(stop);
     stopping_point = stop.tofloat();
 }
 
 /// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
-/// subtract_pos_offsets should be true if the stopping point will be used as a position target (because the pos controller will add back the offsets)
-void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point, bool subtract_pos_offsets) const
+void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
 {
     Vector3p stop;
-    _pos_control.get_stopping_point_xy_cm(stop.xy(), subtract_pos_offsets);
+    _pos_control.get_stopping_point_xy_cm(stop.xy());
     _pos_control.get_stopping_point_z_cm(stop.z);
     stopping_point = stop.tofloat();
 }
@@ -472,10 +470,11 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     _pos_control.update_pos_offset_z(terr_offset);
 
     // get position controller's position offset (post input shaping) so it can be used in position error calculation
-    const Vector3p& psc_pos_offset = _pos_control.get_pos_offset_cm();
+    const Vector3p& psc_pos_offset_target = _pos_control.get_pos_offset_target_cm();
 
     // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
-    const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{psc_pos_offset.tofloat().x, psc_pos_offset.tofloat().y, terr_offset};
+    // question: shouldn't this be a Vector3p
+    const Vector3f &curr_pos = _inav.get_position_neu_cm() - psc_pos_offset_target.tofloat();
     Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
     curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
 
@@ -510,6 +509,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);
 
     // target position, velocity and acceleration from straight line or spline calculators
+// question: doesn't target_pos need to be Vector3p
     Vector3f target_pos, target_vel, target_accel;
 
     bool s_finished;
@@ -534,13 +534,9 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     target_accel *= sq(vel_scaler_dt);
     target_accel += accel_offset;
 
-    // convert final_target.z to altitude above the ekf origin
-    target_pos += _pos_control.get_pos_offset_cm().tofloat();
-    target_vel += _pos_control.get_vel_offset_cms();
-    target_accel += _pos_control.get_accel_offset_cmss();
-
     // pass new target to the position controller
     _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);
+    _pos_control.add_offsets_xy();
 
     // check if we've reached the waypoint
     if (!_flags.reached_destination) {

libraries/AC_WPNav/AC_WPNav.h

@@ -136,10 +136,8 @@ class AC_WPNav
     void shift_wp_origin_and_destination_to_stopping_point_xy();
 
     /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
-    /// subtract_pos_offsets should be true if the stopping point will be used as a position target (because the pos controller will add back the offsets)
-    ///		results placed in stopping_position vector
-    void get_wp_stopping_point_xy(Vector2f& stopping_point, bool subtract_pos_offsets = false) const;
-    void get_wp_stopping_point(Vector3f& stopping_point, bool subtract_pos_offsets = false) const;
+    void get_wp_stopping_point_xy(Vector2f& stopping_point) const;
+    void get_wp_stopping_point(Vector3f& stopping_point) const;
 
     /// get_wp_distance_to_destination - get horizontal distance to destination in cm
     virtual float get_wp_distance_to_destination() const;