squash

lthall · Sep 16, 2024 · e42581a · e42581a
1 parent f2e57af
commit e42581a
Show file tree

Hide file tree

Showing 5 changed files with 6 additions and 6 deletions.
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -170,7 +170,7 @@ class Mode {
     // altitude fence
     float get_avoidance_adjusted_climbrate(float target_rate);
 
-    const Vector3f& get_vel_desired_cms() {
+    Vector3f get_vel_desired_cms() {
         // note that position control isn't used in every mode, so
         // this may return bogus data:
         return pos_control->get_vel_desired_cms();

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1124,6 +1124,8 @@ void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel,
     _pos_target = pos;
     _vel_desired = vel;
     _accel_desired = accel;
+    update_xy_offsets();
+    add_offsets_xy();
 }
 
 /// set position, velocity and acceleration targets
@@ -1132,6 +1134,8 @@ void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& ve
     _pos_target.xy() = pos;
     _vel_desired.xy() = vel;
     _accel_desired.xy() = accel;
+    update_xy_offsets();
+    add_offsets_xy();
 }
 
 // get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -291,7 +291,7 @@ class AC_PosControl
     void set_vel_desired_xy_cms(const Vector2f &vel) {_vel_desired.xy() = vel; }
 
     /// get_vel_desired_cms - returns desired velocity (i.e. feed forward) in cm/s in NEU
-    const Vector3f& get_vel_desired_cms() { return _vel_desired; }
+    Vector3f get_vel_desired_cms() { return _vel_desired - _vel_offset; }
 
     // get_vel_target_cms - returns the target velocity (not including offsets) in NEU cm/s
     Vector3f get_vel_target_cms() const { return _vel_target - _vel_offset; }

diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp
@@ -189,9 +189,7 @@ 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();
 }
 

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -466,7 +466,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);
 
     // input shape the horizontal and terrain offsets
-    _pos_control.update_xy_offsets();
     _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
@@ -536,7 +535,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
 
     // 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) {