AC_WPNav: Loiter: Real time S-Curve Command Model

Author: lthall

libraries/AC_WPNav/AC_Loiter.cpp

@@ -135,6 +135,8 @@ void AC_Loiter::init_target()
     _predicted_accel_ne_mss = _pos_control.get_accel_target_NED_mss().xy();
     _predicted_euler_angle_rad.x = _pos_control.get_roll_rad();
     _predicted_euler_angle_rad.y = _pos_control.get_pitch_rad();
+    _predicted_euler_rate.zero();
+    _predicted_euler_accel.zero();
     _brake_accel_mss = 0.0f;
 }
 
@@ -171,7 +173,7 @@ void AC_Loiter::set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, f
     Vector2f angle_error_euler_rad(wrap_PI(euler_roll_angle_rad - _predicted_euler_angle_rad.x), wrap_PI(euler_pitch_angle_rad - _predicted_euler_angle_rad.y));
 
     // Predict roll/pitch rate required to achieve target attitude
-    _attitude_control.input_shaping_rate_predictor(angle_error_euler_rad, _predicted_euler_rate, dt_s);
+    _attitude_control.input_shaping_rate_predictor(angle_error_euler_rad, _predicted_euler_rate, _predicted_euler_accel, dt_s);
 
     // Update internal attitude estimate for next iteration
     _predicted_euler_angle_rad += _predicted_euler_rate * dt_s;

libraries/AC_WPNav/AC_Loiter.h

@@ -130,6 +130,7 @@ class AC_Loiter
     Vector2f    _predicted_accel_ne_mss;    // Predicted acceleration in m/s² based on internal rate shaping of pilot input.
     Vector2f    _predicted_euler_angle_rad; // Predicted roll/pitch angles (in radians) used for rate shaping of pilot input.
     Vector2f    _predicted_euler_rate;      // Predicted roll/pitch angular rates (in rad/s) for pilot acceleration shaping.
+    Vector2f    _predicted_euler_accel;     // Predicted roll/pitch angular rates (in rad/s) for pilot acceleration shaping.
     uint32_t    _brake_timer_ms;            // Timestamp (in ms) when braking logic was last triggered (sticks released).
     float       _brake_accel_mss;           // Current braking acceleration in m/s², updated using jerk limits over time.
 };