AC_WPNav: Fix before squash

libraries/AC_WPNav/AC_Circle.cpp

@@ -156,7 +156,7 @@ bool AC_Circle::update(float climb_rate_cms)
     calc_velocities(false);
 
     // calculate dt
-    float dt = _pos_control.get_dt();
+    const float dt = _pos_control.get_dt();
 
     // ramp angular velocity to maximum
     if (_angular_vel < _angular_vel_max) {
@@ -189,12 +189,15 @@ bool AC_Circle::update(float climb_rate_cms)
     }
 
     // if the circle_radius is zero we are doing panorama so no need to update loiter target
-    Vector3f target;
+    Vector3f target {
+        _center.x,
+        _center.y,
+        target_z_cm
+    };
     if (!is_zero(_radius)) {
         // calculate target position
-        target.x = _center.x + _radius * cosf(-_angle);
-        target.y = _center.y - _radius * sinf(-_angle);
-        target.z = target_z_cm;
+        target.x += _radius * cosf(-_angle);
+        target.y += - _radius * sinf(-_angle);
 
         // heading is from vehicle to center of circle
         _yaw = get_bearing_cd(_inav.get_position(), _center);
@@ -205,20 +208,15 @@ bool AC_Circle::update(float climb_rate_cms)
         }
 
     } else {
-        // set target position to center
-        target.x = _center.x;
-        target.y = _center.y;
-        target.z = target_z_cm;
-
         // heading is same as _angle but converted to centi-degrees
         _yaw = _angle * DEGX100;
     }
 
     // update position controller target
-    Vector3f vel = Vector3f();
-    _pos_control.input_pos_vel_accel_xy(target, vel, Vector3f());
+    Vector3f zero;
+    _pos_control.input_pos_vel_accel_xy(target, zero, zero);
     if(_terrain_alt) {
-        _pos_control.input_pos_vel_accel_z(target, vel, Vector3f());
+        _pos_control.input_pos_vel_accel_z(target, zero, zero);
     } else {
         _pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms,  false);
     }

libraries/AC_WPNav/AC_Loiter.cpp

@@ -133,21 +133,21 @@ void AC_Loiter::soften_for_landing()
 
     // also prevent I term build up in xy velocity controller. Note
     // that this flag is reset on each loop, in update_xy_controller()
-    _pos_control.set_limit_xy();
+    _pos_control.set_externally_limited_xy();
 }
 
 /// set pilot desired acceleration in centi-degrees
 //   dt should be the time (in seconds) since the last call to this function
 void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd)
 {
-    float dt = _pos_control.get_dt();
+    const float dt = _pos_control.get_dt();
     // Convert from centidegrees on public interface to radians
     const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
     const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
 
     // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically
-    Vector3f desired_euler = Vector3f{euler_roll_angle, euler_pitch_angle, _ahrs.yaw};
-    Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler);
+    const Vector3f desired_euler {euler_roll_angle, euler_pitch_angle, _ahrs.yaw};
+    const Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler);
 
     _desired_accel.x = desired_accel.x;
     _desired_accel.y = desired_accel.y;
@@ -162,8 +162,8 @@ void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float
     _predicted_euler_angle += _predicted_euler_rate * dt;
 
     // convert our predicted attitude to an acceleration vector assuming we are not accelerating vertically
-    Vector3f predicted_euler = Vector3f{_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw};
-    Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler);
+    const Vector3f predicted_euler {_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw};
+    const Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler);
 
     _predicted_accel.x = predicted_accel.x;
     _predicted_accel.y = predicted_accel.y;
@@ -187,13 +187,10 @@ float AC_Loiter::get_angle_max_cd() const
 /// run the loiter controller
 void AC_Loiter::update(bool avoidance_on)
 {
-    // calculate dt
-    float dt = _pos_control.get_dt();
-
     // initialise pos controller speed and acceleration
     _pos_control.set_max_speed_accel_xy(LOITER_VEL_CORRECTION_MAX, _accel_cmss, _accel_cmss);
 
-    calc_desired_velocity(dt, avoidance_on);
+    calc_desired_velocity(_pos_control.get_dt(), avoidance_on);
     _pos_control.update_xy_controller();
 }
 
@@ -228,7 +225,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
 
     // get loiters desired velocity from the position controller where it is being stored.
     const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms();
-    Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);
+    Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y};
 
     // update the desired velocity using our predicted acceleration
     desired_vel.x += _predicted_accel.x * nav_dt;
@@ -289,11 +286,10 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
 
     // get loiters desired velocity from the position controller where it is being stored.
     const Vector3f &target_pos_3d = _pos_control.get_pos_target_cm();
-    Vector2f target_pos{target_pos_3d.x,target_pos_3d.y};
+    Vector2f target_pos{target_pos_3d.x, target_pos_3d.y};
 
     // update the target position using our predicted velocity
-    target_pos.x += desired_vel.x * nav_dt;
-    target_pos.y += desired_vel.y * nav_dt;
+    target_pos += desired_vel * nav_dt;
 
     // send adjusted feed forward acceleration and velocity back to the Position Controller
     _pos_control.set_pos_vel_accel_xy(target_pos, desired_vel, _desired_accel);

libraries/AC_WPNav/AC_WPNav.cpp

@@ -365,7 +365,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
     _pos_control.init_xy_controller();
 
     // get current and target locations
-    const Vector3f& curr_pos = _pos_control.get_pos_target_cm();
+    const Vector3f& curr_pos = _inav.get_position();
 
     // shift origin and destination horizontally
     _origin.x = curr_pos.x;
@@ -538,11 +538,8 @@ bool AC_WPNav::update_wpnav()
 {
     bool ret = true;
 
-    // get dt from pos controller
-    float dt = _pos_control.get_dt();
-
     // advance the target if necessary
-    if (!advance_wp_target_along_track(dt)) {
+    if (!advance_wp_target_along_track(_pos_control.get_dt())) {
         // To-Do: handle inability to advance along track (probably because of missing terrain data)
         ret = false;
     }