Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AP_Param/AP_Param.h>
       5             : #include <AP_Math/AP_Math.h>
       6             : #include <AP_Common/Location.h>
       7             : #include <AC_AttitudeControl/AC_PosControl.h>
       8             : #include <AC_AttitudeControl/AC_AttitudeControl.h>
       9             : #include <AC_Avoidance/AC_Avoid.h>
      10             : 
      11             : class AC_Loiter
      12             : {
      13             : public:
      14             : 
      15             :     /// Constructor
      16             :     AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
      17             : 
      18             :     // Sets the initial loiter target position in meters from the EKF origin.
      19             :     // - position_ne_m: horizontal position in the NE frame, in meters.
      20             :     // - Initializes internal control state including acceleration targets and feed-forward planning.
      21             :     void init_target_m(const Vector2p& position_ne_m);
      22             : 
      23             :     // Initializes the loiter controller using the current position and velocity.
      24             :     // Updates feed-forward velocity, predicted acceleration, and resets control state.
      25             :     void init_target();
      26             : 
      27             :     // Reduces loiter responsiveness for smoother descent during landing.
      28             :     // Internally softens horizontal control gains.
      29             :     void soften_for_landing();
      30             : 
      31             :     // Sets pilot desired acceleration using Euler angles in centidegrees.
      32             :     // See set_pilot_desired_acceleration_rad() for full details.
      33             :     void set_pilot_desired_acceleration_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd);
      34             : 
      35             :     // Sets pilot desired acceleration using Euler angles in radians.
      36             :     // - Internally computes a smoothed acceleration vector based on predictive rate shaping.
      37             :     // - Inputs: `euler_roll_angle_rad`, `euler_pitch_angle_rad` in radians.
      38             :     // - Applies internal shaping using the current attitude controller dt.
      39             :     void set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad);
      40             : 
      41             :     // Returns pilot-requested horizontal acceleration in the NE frame in m/s².
      42             :     // This is the internally computed and smoothed acceleration vector applied by the loiter controller.
      43       36129 :     const Vector2f& get_pilot_desired_acceleration_NE_mss() const { return _desired_accel_ne_mss; }
      44             : 
      45             :     // Clears any pilot-requested acceleration by setting roll and pitch inputs to zero.
      46      412991 :     void clear_pilot_desired_acceleration() { set_pilot_desired_acceleration_rad(0.0, 0.0); }
      47             : 
      48             :     // Calculates the expected stopping point based on current velocity and position in the NE frame.
      49             :     // Result is returned in meters.
      50             :     // Uses the position controller’s deceleration model.
      51             :     void get_stopping_point_NE_m(Vector2f& stopping_point_ne_m) const;
      52             : 
      53             :     // Returns the horizontal distance to the loiter target in meters.
      54             :     // Computed using the NE position error from the position controller.
      55       51926 :     float get_distance_to_target_m() const { return _pos_control.get_pos_error_NE_m(); }
      56             : 
      57             :     // Returns the bearing from current position to the loiter target in radians.
      58             :     // Bearing is relative to true north, using the NE frame.
      59       51926 :     float get_bearing_to_target_rad() const { return _pos_control.get_bearing_to_target_rad(); }
      60             : 
      61             :     // Returns the maximum pilot-commanded lean angle in centidegrees.
      62             :     // See get_angle_max_rad() for full details.
      63             :     float get_angle_max_cd() const;
      64             : 
      65             :     // Returns the maximum pilot-commanded lean angle in radians.
      66             :     // - If `_angle_max_deg` is zero, this returns 2/3 of the limiting PSC angle.
      67             :     // - Otherwise, returns the minimum of `_angle_max_deg` and PSC’s configured angle limit.
      68             :     float get_angle_max_rad() const;
      69             : 
      70             :     // Runs the loiter control loop, computing desired acceleration and updating position control.
      71             :     // If `avoidance_on` is true, velocity is adjusted using avoidance logic before being applied.
      72             :     void update(bool avoidance_on = true);
      73             : 
      74             :     // Sets the maximum allowed horizontal loiter speed in m/s.
      75             :     void set_speed_max_NE_ms(float speed_max_NE_ms);
      76             : 
      77             :     // Returns the desired roll angle in centidegrees from the loiter controller.
      78      636619 :     float get_roll_cd() const { return rad_to_cd(get_roll_rad()); }
      79             : 
      80             :     // Returns the desired pitch angle in centidegrees from the loiter controller.
      81      636619 :     float get_pitch_cd() const { return rad_to_cd(get_pitch_rad()); }
      82             : 
      83             :     // Returns the desired roll angle in radians from the loiter controller.
      84      796525 :     float get_roll_rad() const { return _pos_control.get_roll_rad(); }
      85             : 
      86             :     // Returns the desired pitch angle in radians from the loiter controller.
      87      796525 :     float get_pitch_rad() const { return _pos_control.get_pitch_rad(); }
      88             : 
      89             :     // Returns the desired 3D thrust vector from the loiter controller for attitude control.
      90             :     // Directional only; magnitude is handled by the attitude controller.
      91     3459501 :     Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); }
      92             : 
      93             :     // perform any required parameter conversions
      94             :     void convert_parameters();
      95             : 
      96             :     static const struct AP_Param::GroupInfo var_info[];
      97             : 
      98             : protected:
      99             : 
     100             :     // Ensures internal parameters are within valid safety limits.
     101             :     // Applies min/max constraints on speed and acceleration settings.
     102             :     void sanity_check_params();
     103             : 
     104             :     // Updates feed-forward velocity using pilot-requested acceleration and braking logic.
     105             :     // - Applies drag and braking forces when sticks are released.
     106             :     // - Velocity is adjusted for fence/avoidance if enabled.
     107             :     // - Resulting velocity and acceleration are sent to the position controller.
     108             :     void calc_desired_velocity(bool avoidance_on = true);
     109             : 
     110             :     // references and pointers to external libraries
     111             :     const AP_AHRS_View&     _ahrs;
     112             :     AC_PosControl&          _pos_control;
     113             :     const AC_AttitudeControl& _attitude_control;
     114             : 
     115             :     // parameters
     116             :     AP_Float    _angle_max_deg;         // Maximum pilot-commanded lean angle in degrees. Set to zero to default to 2/3 of PSC_ANGLE_MAX (or Q_ANGLE_MAX for QuadPlane).
     117             :     AP_Float    _speed_max_ne_ms;       // Maximum horizontal speed in m/s while in loiter mode. Used to limit both user and internal trajectory velocities.
     118             :     AP_Float    _accel_max_ne_mss;      // Maximum horizontal acceleration (in m/s²) applied during normal loiter corrections.
     119             :     AP_Float    _brake_accel_max_mss;   // Maximum braking acceleration (in m/s²) applied when pilot sticks are released.
     120             :     AP_Float    _brake_jerk_max_msss;   // Maximum braking jerk (in m/s³) applied during braking transitions after pilot release.
     121             :     AP_Float    _brake_delay_s;         // Delay in seconds before braking begins after sticks are centered. Prevents premature deceleration during brief pauses.
     122             :     AP_Int8     _options;               // Loiter options bit mask
     123             : 
     124             :     // Bitfields of LOITER_OPTIONS
     125             :     enum class LoiterOption {
     126             :         COORDINATED_TURN_ENABLED    = (1U << 0),    // Enable Coordinated Turn
     127             :     };
     128             :     bool loiter_option_is_set(LoiterOption option) const;
     129             : 
     130             :     // loiter controller internal variables
     131             :     Vector2f    _desired_accel_ne_mss;      // Pilot-requested horizontal acceleration in m/s² (after smoothing), in the NE (horizontal) frame.
     132             :     Vector2f    _predicted_accel_ne_mss;    // Predicted acceleration in m/s² based on internal rate shaping of pilot input.
     133             :     Vector2f    _predicted_euler_angle_rad; // Predicted roll/pitch angles (in radians) used for rate shaping of pilot input.
     134             :     Vector2f    _predicted_euler_rate;      // Predicted roll/pitch angular rates (in rad/s) for pilot acceleration shaping.
     135             :     Vector2f    _predicted_euler_accel;     // Predicted roll/pitch angular rates (in rad/s) for pilot acceleration shaping.
     136             :     uint32_t    _brake_timer_ms;            // Timestamp (in ms) when braking logic was last triggered (sticks released).
     137             :     float       _brake_accel_mss;           // Current braking acceleration in m/s², updated using jerk limits over time.
     138             : };