Line data    Source code

       1             : #pragma once
       2             : #if AP_SCRIPTING_ENABLED
       3             : 
       4             : #include "AC_AttitudeControl_Multi.h"
       5             : 
       6             : class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
       7             : public:
       8           1 :     AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, AP_MotorsMulticopter& motors):
       9           1 :         AC_AttitudeControl_Multi(ahrs,motors) {
      10             : 
      11           1 :         if (_singleton != nullptr) {
      12           0 :             AP_HAL::panic("Can only be one AC_AttitudeControl_Multi_6DoF");
      13             :         }
      14           1 :         _singleton = this;
      15           1 :     }
      16             : 
      17           2 :     static AC_AttitudeControl_Multi_6DoF *get_singleton() {
      18           2 :         return _singleton;
      19             :     }
      20             : 
      21             :     // Sets a desired attitude using a quaternion and body-frame angular velocity (rad/s).
      22             :     // The desired quaternion is incrementally updated each timestep. Angular velocity is shaped by acceleration limits and feedforward.
      23             :     void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) override;
      24             :     /*
      25             :         override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles
      26             :     */
      27             : 
      28             :     // Sets desired roll and pitch angles (in radians) and yaw rate (in radians/s).
      29             :     // Used when roll/pitch stabilization is needed with manual or autonomous yaw rate control.
      30             :     // Applies acceleration-limited input shaping for smooth transitions and computes body-frame angular velocity targets.
      31             :     void input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads)  override;
      32             : 
      33             : 
      34             :     // Sets desired roll, pitch, and yaw angles (in radians).
      35             :     // Used to follow an absolute attitude setpoint. Input shaping and yaw slew limits are applied.
      36             :     // Outputs are passed to the rate controller via shaped angular velocity targets.
      37             :     void input_euler_angle_roll_pitch_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_angle_rad, bool slew_yaw) override;
      38             : 
      39             :     // Sets desired thrust vector and heading rate (in radians/s).
      40             :     // Used for tilt-based navigation with independent yaw control.
      41             :     // The thrust vector defines the desired orientation (e.g., pointing direction for vertical thrust),
      42             :     // while the heading rate adjusts yaw. The input is shaped by acceleration and slew limits.
      43             :     void input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw = true) override;
      44             :     
      45             :     // Sets desired thrust vector and heading (in radians) with heading rate (in radians/s).
      46             :     // Used for advanced attitude control where thrust direction is separated from yaw orientation.
      47             :     // Heading slew is constrained based on configured limits.
      48             :     void input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads) override;
      49             : 
      50             :     /*
      51             :         all other input functions should zero thrust vectoring and behave as a normal copter
      52             :     */
      53             : 
      54             :     // Command euler yaw rate and pitch angle with roll angle specified in body frame
      55             :     // (used only by tailsitter quadplanes)
      56             :     void input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(bool plane_controls, float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads) override;
      57             : 
      58             :     // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
      59             :     void input_euler_rate_roll_pitch_yaw_rads(float euler_roll_rate_rads, float euler_pitch_rate_rads, float euler_yaw_rate_rads) override;
      60             : 
      61             : 
      62             :     // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
      63             :     // This command is used by fully stabilized acro modes.
      64             :     // It applies angular velocity targets in the body frame,
      65             :     // shaped using acceleration limits and passed to the rate controller.
      66             :     void input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) override;
      67             : 
      68             :     // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
      69             :     // Used by Copter's rate-only acro mode.
      70             :     // Applies raw angular velocity targets directly to the rate controller with smoothing
      71             :     // and no attitude feedback or stabilization.
      72             :     void input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) override;
      73             : 
      74             : 
      75             :     // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
      76             :     // Used by Plane's acro mode with rate error integration.
      77             :     // Integrates attitude error over time to generate target angular rates.
      78             :     void input_rate_bf_roll_pitch_yaw_3_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) override;
      79             : 
      80             :     // Applies a one-time angular offset in body-frame roll/pitch/yaw angles (in radians).
      81             :     // Used for initiating step responses during autotuning or manual test inputs.
      82             :     void input_angle_step_bf_roll_pitch_yaw_rad(float roll_angle_step_bf_rad, float pitch_angle_step_bf_rad, float yaw_angle_step_bf_rad) override;
      83             : 
      84             :     // run lowest level body-frame rate controller and send outputs to the motors
      85             :     void rate_controller_run() override;
      86             : 
      87             :     // limiting lean angle based on throttle makes no sense for 6DoF, always allow 90 deg, return in centi-degrees
      88       33423 :     float get_althold_lean_angle_max_rad() const override { return radians(90.0f); }
      89             : 
      90             :     // set the attitude that will be used in 6DoF flight
      91           0 :     void set_offset_roll_pitch(float roll_deg, float pitch_deg) {
      92           0 :         roll_offset_deg = roll_deg;
      93           0 :         pitch_offset_deg = pitch_deg;
      94           0 :     }
      95             : 
      96             :     // these flags enable or disable lateral or forward thrust, with both disabled the vehicle acts like a traditional copter
      97             :     // it will roll and pitch to move, its also possible to enable only forward or lateral to suit the vehicle configuration.
      98             :     // by default the vehicle is full 6DoF, these can be set in flight via scripting
      99           1 :     void set_forward_enable(bool b) {
     100           1 :         forward_enable = b;
     101           1 :     }
     102           1 :     void set_lateral_enable(bool b) {
     103           1 :         lateral_enable = b;
     104           1 :     }
     105             : 
     106             : private:
     107             : 
     108             :     void set_forward_lateral_rad(float &euler_pitch_angle_rad, float &euler_roll_angle_rad);
     109             : 
     110             :     float roll_offset_deg;
     111             :     float pitch_offset_deg;
     112             : 
     113             :     bool forward_enable = true;
     114             :     bool lateral_enable = true;
     115             : 
     116             :     static AC_AttitudeControl_Multi_6DoF *_singleton;
     117             : 
     118             : };
     119             : 
     120             : #endif // AP_SCRIPTING_ENABLED