Line data Source code
1 : #include <AP_Scripting/AP_Scripting_config.h>
2 :
3 : #if AP_SCRIPTING_ENABLED
4 :
5 : #include "AC_AttitudeControl_Multi_6DoF.h"
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <AP_Math/AP_Math.h>
8 :
9 : // 6DoF control is extracted from the existing copter code by treating desired angles as thrust angles rather than vehicle attitude.
10 : // Vehicle attitude is then set separately, typically the vehicle would maintain 0 roll and pitch.
11 : // rate commands result in the vehicle behaving as a ordinary copter.
12 :
13 : // run lowest level body-frame rate controller and send outputs to the motors
14 49958 : void AC_AttitudeControl_Multi_6DoF::rate_controller_run() {
15 :
16 : // pass current offsets to motors and run baseclass controller
17 : // motors require the offsets to know which way is up
18 49958 : float roll_deg = roll_offset_deg;
19 49958 : float pitch_deg = pitch_offset_deg;
20 : // if 6DoF control, always point directly up
21 : // this stops horizontal drift due to error between target and true attitude
22 49958 : if (lateral_enable) {
23 1 : roll_deg = AP::ahrs().get_roll_deg();
24 : }
25 49958 : if (forward_enable) {
26 1 : pitch_deg = AP::ahrs().get_pitch_deg();
27 : }
28 49958 : _motors.set_roll_pitch(roll_deg,pitch_deg);
29 :
30 49958 : AC_AttitudeControl_Multi::rate_controller_run();
31 49958 : }
32 :
33 : /*
34 : override all input to the attitude controller and convert desired angles into thrust angles and substitute
35 : */
36 :
37 : // Sets desired roll and pitch angles (in radians) and yaw rate (in radians/s).
38 : // Used when roll/pitch stabilization is needed with manual or autonomous yaw rate control.
39 : // Applies acceleration-limited input shaping for smooth transitions and computes body-frame angular velocity targets.
40 42204 : void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads) {
41 :
42 42204 : set_forward_lateral_rad(euler_pitch_angle_rad, euler_roll_angle_rad);
43 :
44 42204 : AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_euler_rate_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_rate_rads);
45 42204 : }
46 :
47 :
48 : // Sets desired roll, pitch, and yaw angles (in radians).
49 : // Used to follow an absolute attitude setpoint. Input shaping and yaw slew limits are applied.
50 : // Outputs are passed to the rate controller via shaped angular velocity targets.
51 6664 : void AC_AttitudeControl_Multi_6DoF::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) {
52 :
53 6664 : set_forward_lateral_rad(euler_pitch_angle_rad, euler_roll_angle_rad);
54 :
55 6664 : AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_angle_rad, slew_yaw);
56 6664 : }
57 :
58 : // Sets desired thrust vector and heading rate (in radians/s).
59 : // Used for tilt-based navigation with independent yaw control.
60 : // The thrust vector defines the desired orientation (e.g., pointing direction for vertical thrust),
61 : // while the heading rate adjusts yaw. The input is shaped by acceleration and slew limits.
62 24871 : void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw)
63 : {
64 : // convert thrust vector to a roll and pitch angles
65 : // this negates the advantage of using thrust vector control, but works just fine
66 24871 : Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
67 :
68 24871 : input_euler_angle_roll_pitch_euler_rate_yaw_rad(angle_target.x, angle_target.y, heading_rate_rads);
69 24871 : }
70 :
71 : // Sets desired thrust vector and heading (in radians) with heading rate (in radians/s).
72 : // Used for advanced attitude control where thrust direction is separated from yaw orientation.
73 : // Heading slew is constrained based on configured limits.
74 6664 : void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads)
75 : {
76 : // convert thrust vector to a roll and pitch angles
77 6664 : Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
78 :
79 : // note that we are throwing away heading rate here
80 6664 : input_euler_angle_roll_pitch_yaw_rad(angle_target.x, angle_target.y, heading_angle_rad, true);
81 6664 : }
82 :
83 48868 : void AC_AttitudeControl_Multi_6DoF::set_forward_lateral_rad(float &euler_pitch_angle_rad, float &euler_roll_angle_rad)
84 : {
85 : // pitch/forward
86 48868 : if (forward_enable) {
87 1 : _motors.set_forward(-sinf(euler_pitch_angle_rad));
88 1 : euler_pitch_angle_rad = radians(pitch_offset_deg);
89 : } else {
90 48867 : _motors.set_forward(0.0f);
91 48867 : euler_pitch_angle_rad += radians(pitch_offset_deg);
92 : }
93 48868 : euler_pitch_angle_rad = wrap_PI(euler_pitch_angle_rad);
94 :
95 : // roll/lateral
96 48868 : if (lateral_enable) {
97 1 : _motors.set_lateral(sinf(euler_roll_angle_rad));
98 1 : euler_roll_angle_rad = radians(roll_offset_deg);
99 : } else {
100 48867 : _motors.set_lateral(0.0f);
101 48867 : euler_roll_angle_rad += radians(roll_offset_deg);
102 : }
103 48868 : euler_roll_angle_rad = wrap_PI(euler_roll_angle_rad);
104 48868 : }
105 :
106 : /*
107 : all other input functions should zero thrust vectoring
108 : */
109 :
110 : // Command euler yaw rate and pitch angle with roll angle specified in body frame
111 : // (used only by tailsitter quadplanes)
112 0 : void AC_AttitudeControl_Multi_6DoF::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) {
113 0 : _motors.set_lateral(0.0f);
114 0 : _motors.set_forward(0.0f);
115 :
116 0 : AC_AttitudeControl_Multi::input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(plane_controls, euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_rate_rads);
117 0 : }
118 :
119 : // Sets desired roll, pitch, and yaw angular rates (in radians/s).
120 : // This command is used to apply angular rate targets in the earth frame.
121 : // The inputs are shaped using acceleration limits and time constants.
122 : // Resulting targets are converted into body-frame angular velocities
123 : // and passed to the rate controller.
124 0 : void AC_AttitudeControl_Multi_6DoF::input_euler_rate_roll_pitch_yaw_rads(float euler_roll_rate_rads, float euler_pitch_rate_rads, float euler_yaw_rate_rads) {
125 0 : _motors.set_lateral(0.0f);
126 0 : _motors.set_forward(0.0f);
127 :
128 0 : AC_AttitudeControl_Multi::input_euler_rate_roll_pitch_yaw_rads(euler_roll_rate_rads, euler_pitch_rate_rads, euler_yaw_rate_rads);
129 0 : }
130 :
131 :
132 : // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
133 : // This command is used by fully stabilized acro modes.
134 : // It applies angular velocity targets in the body frame,
135 : // shaped using acceleration limits and passed to the rate controller.
136 0 : void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) {
137 0 : _motors.set_lateral(0.0f);
138 0 : _motors.set_forward(0.0f);
139 :
140 0 : AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
141 0 : }
142 :
143 : // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
144 : // Used by Copter's rate-only acro mode.
145 : // Applies raw angular velocity targets directly to the rate controller with smoothing
146 : // and no attitude feedback or stabilization.
147 0 : void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) {
148 0 : _motors.set_lateral(0.0f);
149 0 : _motors.set_forward(0.0f);
150 :
151 0 : AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw_2_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
152 0 : }
153 :
154 : // Sets desired roll, pitch, and yaw angular rates in body-frame (in radians/s).
155 : // Used by Plane's acro mode with rate error integration.
156 : // Integrates attitude error over time to generate target angular rates.
157 0 : void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw_3_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) {
158 0 : _motors.set_lateral(0.0f);
159 0 : _motors.set_forward(0.0f);
160 :
161 0 : AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw_3_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
162 0 : }
163 :
164 : // Applies a one-time angular offset in body-frame roll/pitch/yaw angles (in radians).
165 : // Used for initiating step responses during autotuning or manual test inputs.
166 0 : void AC_AttitudeControl_Multi_6DoF::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) {
167 0 : _motors.set_lateral(0.0f);
168 0 : _motors.set_forward(0.0f);
169 :
170 0 : AC_AttitudeControl_Multi::input_angle_step_bf_roll_pitch_yaw_rad(roll_angle_step_bf_rad, pitch_angle_step_bf_rad, yaw_angle_step_bf_rad);
171 0 : }
172 :
173 : // Sets a desired attitude using a quaternion and body-frame angular velocity (rad/s).
174 : // The desired quaternion is incrementally updated each timestep. Angular velocity is shaped by acceleration limits and feedforward.
175 0 : void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) {
176 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
177 0 : AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
178 : #endif
179 :
180 : _motors.set_lateral(0.0f);
181 : _motors.set_forward(0.0f);
182 :
183 : AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_body);
184 : }
185 :
186 :
187 : AC_AttitudeControl_Multi_6DoF *AC_AttitudeControl_Multi_6DoF::_singleton = nullptr;
188 :
189 : #endif // AP_SCRIPTING_ENABLED
|
