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 <AC_AttitudeControl/AC_PosControl.h>      // Position control library
       7             : 
       8             : class AC_Circle
       9             : {
      10             : public:
      11             : 
      12             :     /// Constructor
      13             :     AC_Circle(const AP_AHRS_View& ahrs, AC_PosControl& pos_control);
      14             : 
      15             :     // Initializes circle flight using a center position in centimeters relative to the EKF origin.
      16             :     // See init_NED_m() for full details.
      17             :     void init_NEU_cm(const Vector3p& center_neu_cm, bool is_terrain_alt, float rate_degs);
      18             : 
      19             :     // Initializes circle flight mode using a specified center position in meters.
      20             :     // Parameters:
      21             :     //  - center_ned_m: Center of the circle in NED frame (meters, relative to EKF origin)
      22             :     // - is_terrain_alt: If true, center_ned_m.z is interpreted as relative to terrain; otherwise, above EKF origin
      23             :     //  - rate_degs: Desired turn rate in degrees per second (positive = clockwise, negative = counter-clockwise)
      24             :     // Caller must preconfigure the position controller's speed and acceleration settings before calling.
      25             :     void init_NED_m(const Vector3p& center_ned_m, bool is_terrain_alt, float rate_degs);
      26             : 
      27             :     // Initializes the circle flight mode using the current stopping point as a reference.
      28             :     // If the INIT_AT_CENTER option is not set, the circle center is projected one radius ahead along the vehicle's heading.
      29             :     // Caller must configure position controller speeds and accelerations beforehand.
      30             :     void init();
      31             : 
      32             :     // Sets the circle center using a Location object.
      33             :     // Automatically determines whether the location uses terrain-relative or origin-relative altitude.
      34             :     // If conversion fails, defaults to current position and logs a navigation error.
      35             :     void set_center(const Location& center);
      36             : 
      37             :     // Sets the circle center using a NED position vector in meters from the EKF origin.
      38             :     // - is_terrain_alt: If true, center_ned_m.z is interpreted as relative to terrain; otherwise, above EKF origin
      39           7 :     void set_center_NED_m(const Vector3p& center_ned_m, bool is_terrain_alt) { _center_ned_m = center_ned_m; _is_terrain_alt = is_terrain_alt; }
      40             : 
      41             :     // Returns the circle center in centimeters from the EKF origin.
      42             :     // See get_center_NED_m() for full details.
      43           4 :     const Vector3p get_center_NEU_cm() const { return Vector3p(_center_ned_m.x, _center_ned_m.y, -_center_ned_m.z) * 100.0; }
      44             : 
      45             :     // Returns the circle center in meters from the EKF origin.
      46             :     // Altitude frame is determined by `center_is_terrain_alt()`.
      47       46679 :     const Vector3p& get_center_NED_m() const { return _center_ned_m; }
      48             : 
      49             :     // Returns true if the circle center altitude is relative to terrain height.
      50          11 :     bool center_is_terrain_alt() const { return _is_terrain_alt; }
      51             : 
      52             :     // Returns the circle radius in centimeters.
      53             :     // See get_radius_m() for full details.
      54           2 :     float get_radius_cm() const { return get_radius_m() * 100.0; }
      55             : 
      56             :     // Returns the circle radius in meters.
      57             :     // If `_radius_m` is non-positive, falls back to the RADIUS parameter.
      58      116089 :     float get_radius_m() const { return is_positive(_radius_m) ? _radius_m : _radius_parm_m; }
      59             : 
      60             :     // Sets the circle radius in centimeters.
      61             :     // See set_radius_m() for full details.
      62             :     void set_radius_cm(float radius_cm);
      63             : 
      64             :     // Sets the circle radius in meters.
      65             :     // Radius is constrained to AC_CIRCLE_RADIUS_MAX_M.
      66             :     void set_radius_m(float radius_m);
      67             : 
      68             :     // Returns the configured circle turn rate in degrees per second from the RATE parameter.
      69       15766 :     float get_rate_degs() const { return _rate_parm_degs; }
      70             : 
      71             :     // Returns the commanded turn-rate target in degrees per second.
      72             :     // This is the rate the controller ramps towards (set by the RATE
      73             :     // parameter or set_rate_degs()).
      74       15761 :     float get_rate_target_degs() const { return degrees(_rotation_rate_max_rads); }
      75             : 
      76             :     // Sets the target circle rate in degrees per second.
      77             :     // Positive values result in clockwise rotation; negative for counter-clockwise.
      78             :     void set_rate_degs(float rate_degs);
      79             : 
      80             :     // Returns the total angle, in radians, that the vehicle has traveled along the circular path.
      81      127104 :     float get_angle_total_rad() const { return _angle_total_rad; }
      82             : 
      83             :     // Updates the circle controller using a climb rate in cm/s.
      84             :     // See update_ms() for full implementation details.
      85             :     bool update_cms(float climb_rate_cms = 0.0f) WARN_IF_UNUSED;
      86             : 
      87             :     // Updates the circle controller using a climb rate in m/s.
      88             :     // Computes new angular position, yaw, and vertical trajectory, then updates the position controller.
      89             :     // Returns false if terrain data is required but unavailable.
      90             :     bool update_ms(float climb_rate_ms = 0.0f) WARN_IF_UNUSED;
      91             : 
      92             :     // Returns the desired roll angle in centidegrees from the position controller.
      93             :     // Should be passed to the attitude controller as a stabilization input.
      94      127956 :     float get_roll_cd() const { return rad_to_cd(_pos_control.get_roll_rad()); }
      95             : 
      96             :     // Returns the desired pitch angle in centidegrees from the position controller.
      97             :     // Should be passed to the attitude controller as a stabilization input.
      98      127956 :     float get_pitch_cd() const { return rad_to_cd(_pos_control.get_pitch_rad()); }
      99             : 
     100             :     // Returns the desired yaw angle in centidegrees computed by the circle controller.
     101             :     // May be oriented toward the circle center or along the path depending on configuration.
     102      127956 :     float get_yaw_cd() const { return rad_to_cd(_yaw_rad); }
     103             : 
     104             :     // Returns the desired yaw angle in radians.
     105             :     // Used for directional control based on circle configuration.
     106      109881 :     float get_yaw_rad() const { return _yaw_rad; }
     107             : 
     108             :     // Returns the desired thrust vector (unit vector in body frame) from the position controller.
     109             :     // Can be used by the attitude controller to align thrust direction.
     110             :     Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); }
     111             : 
     112             :     // Returns true if the circle controller's update() function has run recently.
     113             :     // Used by vehicle code to determine if yaw and position outputs are valid.
     114             :     bool is_active() const;
     115             : 
     116             :     // Returns the closest point on the circle to the vehicle's current position in centimeters.
     117             :     // See get_closest_point_on_circle_NED_m() for full details.
     118             :     void get_closest_point_on_circle_NEU_cm(Vector3f& result_neu_cm, float& dist_cm) const;
     119             : 
     120             :     // Returns the closest point on the circle to the vehicle's stopping point in meters.
     121             :     // The result vector is updated with the NED position of the closest point on the circle.
     122             :     // The altitude (z) is set to match the circle center's altitude.
     123             :     // dist_m is updated with the 3D distance to the circle edge from the stopping point.
     124             :     // If the vehicle is at the center, the point directly behind the vehicle (based on yaw) is returned.
     125             :     void get_closest_point_on_circle_NED_m(Vector3p& result_ned_m, float& dist_m) const;
     126             : 
     127             :     // Returns the horizontal distance to the circle target in meters.
     128             :     // Calculated using the position controller’s NE position error norm.
     129        1724 :     float get_distance_to_target_m() const { return _pos_control.get_pos_error_NE_m(); }
     130             : 
     131             :     // Returns the bearing from the vehicle to the circle target in radians.
     132             :     // Bearing is measured clockwise from North (0 = North).
     133        1724 :     float get_bearing_to_target_rad() const { return _pos_control.get_bearing_to_target_rad(); }
     134             : 
     135             :     // Returns true if pilot stick control of circle radius and rate is enabled.
     136             :     // See pilot_control_enabled() for flag logic.
     137      115671 :     bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; }
     138             : 
     139             :     // Returns true if the mount ROI is fixed at the circle center.
     140             :     // See roi_at_center() for flag logic.
     141          22 :     bool roi_at_center() const { return (_options.get() & CircleOptions::ROI_AT_CENTER) != 0; }
     142             : 
     143             :     // Returns true if the vehicle yaw should align with the direction of travel (tangent)
     144             :     // rather than facing the circle center.
     145       46670 :     bool face_direction_of_travel() const { return (_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0; }
     146             : 
     147             :     // Sets rangefinder terrain offset (in centimeters) above EKF origin.
     148             :     // See set_rangefinder_terrain_U_m() for full details.
     149      109016 :     void set_rangefinder_terrain_U_cm(bool use, bool healthy, float terrain_u_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_u_m = terrain_u_cm * 0.01;}
     150             : 
     151             :     // Sets rangefinder terrain offset (in meters) above EKF origin.
     152             :     // Used for terrain-relative altitude tracking during circular flight.
     153    30392688 :     void set_rangefinder_terrain_U_m(bool use, bool healthy, float terrain_u_m) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_u_m = terrain_u_m;}
     154             : 
     155             :     // Checks if the circle radius parameter has changed.
     156             :     // If so, updates internal `_radius_m` and stores the new parameter value.
     157             :     void check_param_change();
     158             : 
     159             :     // perform any required parameter conversions
     160             :     void convert_parameters();
     161             : 
     162             :     static const struct AP_Param::GroupInfo var_info[];
     163             : 
     164             : private:
     165             : 
     166             :     // Calculates angular velocity and acceleration limits based on the configured radius and rate.
     167             :     // Should be called whenever radius or rate changes.
     168             :     // If `init_velocity` is true, resets angular velocity to zero (used on controller startup).
     169             :     void calc_velocities(bool init_velocity);
     170             : 
     171             :     // Sets the initial angle around the circle and resets the accumulated angle.
     172             :     // If `use_heading` is true, uses vehicle heading to initialize angle for minimal yaw motion.
     173             :     // If false, uses position relative to circle center to set angle.
     174             :     void init_start_angle(bool use_heading);
     175             : 
     176             :     // Returns the expected source of terrain data for the circle controller.
     177             :     // Used to determine whether terrain offset comes from rangefinder, terrain database, or is unavailable.
     178             :     enum class TerrainSource {
     179             :         TERRAIN_UNAVAILABLE,
     180             :         TERRAIN_FROM_RANGEFINDER,
     181             :         TERRAIN_FROM_TERRAINDATABASE,
     182             :     };
     183             :     AC_Circle::TerrainSource get_terrain_source() const;
     184             : 
     185             :     // Returns terrain offset in meters above the EKF origin at the current position.
     186             :     // Positive values indicate terrain is above the EKF origin altitude.
     187             :     // Terrain source may be rangefinder or terrain database.
     188             :     bool get_terrain_U_m(float& terrain_u_m);
     189             : 
     190             :     // references to inertial nav and ahrs libraries
     191             :     const AP_AHRS_View&         _ahrs;
     192             :     AC_PosControl&              _pos_control;
     193             : 
     194             :     enum CircleOptions {
     195             :         MANUAL_CONTROL           = 1U << 0, // Enables pilot stick input to adjust circle radius and turn rate.
     196             :         FACE_DIRECTION_OF_TRAVEL = 1U << 1, // Yaw aligns with direction of travel (tangent to circle path).
     197             :         INIT_AT_CENTER           = 1U << 2, // Initializes circle with center at current position (instead of radius ahead).
     198             :         ROI_AT_CENTER            = 1U << 3, // Sets camera mount ROI to circle center during circle mode.
     199             :     };
     200             : 
     201             :     // parameters
     202             :     AP_Float _radius_parm_m;      // Circle radius in meters, loaded from parameters.
     203             :     AP_Float _rate_parm_degs;     // Circle rotation rate in degrees per second, loaded from parameters.
     204             :     AP_Int16 _options;            // Bitmask of CircleOptions (e.g. manual control, ROI at center, etc.).
     205             : 
     206             :     // internal variables
     207             :     Vector3p _center_ned_m;             // Center of the circle in meters from EKF origin (NED frame).
     208             :     float    _radius_m;                 // Current circle radius in meters.
     209             :     float    _rotation_rate_max_rads;   // Requested circle turn rate in rad/s (+ve = CW, -ve = CCW).
     210             :     float    _yaw_rad;                  // Desired yaw heading in radians (typically toward circle center or tangent).
     211             :     float    _angle_rad;                // Current angular position around the circle in radians (0 = due north of center).
     212             :     float    _angle_total_rad;          // Accumulated angle travelled in radians (used for full rotations).
     213             :     float    _angular_vel_rads;         // Current angular velocity in rad/s.
     214             :     float    _angular_vel_max_rads;     // Maximum allowed angular velocity in rad/s.
     215             :     float    _angular_accel_radss;      // Angular acceleration limit in rad/s².
     216             :     uint32_t _last_update_ms;           // Timestamp (in milliseconds) of the last update() call.
     217             :     float    _last_radius_param_m;      // Cached copy of radius parameter (m) to detect parameter changes.
     218             : 
     219             :     // terrain following variables
     220             :     bool  _is_terrain_alt;               // True if _center_ned_m.z is relative to terrain height; false if relative to EKF origin.
     221             :     bool  _rangefinder_available;        // True if rangefinder is available and enabled.
     222             :     bool  _rangefinder_healthy;          // True if rangefinder reading is within valid operating range.
     223             :     float _rangefinder_terrain_u_m; // Terrain height above EKF origin (meters), from rangefinder or terrain database.
     224             : };