Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include <AP_Scripting/AP_Scripting_config.h>
17 :
18 : #if AP_SCRIPTING_ENABLED
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include "AP_MotorsMatrix_6DoF_Scripting.h"
22 : #include <GCS_MAVLink/GCS.h>
23 : #include <SRV_Channel/SRV_Channel.h>
24 : #include <GCS_MAVLink/GCS.h>
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 49960 : void AP_MotorsMatrix_6DoF_Scripting::output_to_motors()
29 : {
30 49960 : switch (_spool_state) {
31 19228 : case SpoolState::SHUT_DOWN:
32 : case SpoolState::GROUND_IDLE:
33 : {
34 : // no output, cant spin up for ground idle because we don't know which way motors should be spining
35 634524 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
36 615296 : if (motor_enabled[i]) {
37 76904 : _actuator[i] = 0.0f;
38 : }
39 : }
40 19228 : break;
41 : }
42 30732 : case SpoolState::SPOOLING_UP:
43 : case SpoolState::THROTTLE_UNLIMITED:
44 : case SpoolState::SPOOLING_DOWN:
45 : // set motor output based on thrust requests
46 1014156 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
47 983424 : if (motor_enabled[i]) {
48 122928 : if (_reversible[i]) {
49 : // revesible motor can provide both positive and negative thrust, +- spin max, spin min does not apply
50 0 : if (is_positive(_thrust_rpyt_out[i])) {
51 0 : _actuator[i] = thr_lin.apply_thrust_curve_and_volt_scaling(_thrust_rpyt_out[i]) * thr_lin.get_spin_max();
52 :
53 0 : } else if (is_negative(_thrust_rpyt_out[i])) {
54 0 : _actuator[i] = -thr_lin.apply_thrust_curve_and_volt_scaling(-_thrust_rpyt_out[i]) * thr_lin.get_spin_max();
55 :
56 : } else {
57 0 : _actuator[i] = 0.0f;
58 : }
59 : } else {
60 : // motor can only provide trust in a single direction, spin min to spin max as 'normal' copter
61 122928 : _actuator[i] = thr_lin.thrust_to_actuator(_thrust_rpyt_out[i]);
62 : }
63 : }
64 : }
65 30732 : break;
66 : }
67 :
68 : // Send to each motor
69 1648680 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
70 1598720 : if (motor_enabled[i]) {
71 199832 : SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _actuator[i] * 4500);
72 : }
73 : }
74 49960 : }
75 :
76 : // output_armed - sends commands to the motors
77 49960 : void AP_MotorsMatrix_6DoF_Scripting::output_armed_stabilizing()
78 : {
79 : uint8_t i; // general purpose counter
80 : float roll_thrust; // roll thrust input value, +/- 1.0
81 : float pitch_thrust; // pitch thrust input value, +/- 1.0
82 : float yaw_thrust; // yaw thrust input value, +/- 1.0
83 : float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
84 : float forward_thrust; // forward thrust input value, +/- 1.0
85 : float right_thrust; // right thrust input value, +/- 1.0
86 :
87 : // note that the throttle, forwards and right inputs are not in bodyframe, they are in the frame of the 'normal' 4DoF copter were pretending to be
88 :
89 : // apply voltage and air pressure compensation
90 49960 : const float compensation_gain = thr_lin.get_compensation_gain(); // compensation for battery voltage and altitude
91 49960 : roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
92 49960 : pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
93 49960 : yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
94 49960 : throttle_thrust = get_throttle() * compensation_gain;
95 :
96 : // scale horizontal thrust with throttle, this mimics a normal copter
97 : // so we don't break the lean angle proportional acceleration assumption made by the position controller
98 49960 : forward_thrust = get_forward() * throttle_thrust;
99 49960 : right_thrust = get_lateral() * throttle_thrust;
100 :
101 :
102 : // set throttle limit flags
103 49960 : if (throttle_thrust <= 0) {
104 20019 : throttle_thrust = 0;
105 : // we cant thrust down, the vehicle can do it, but it would break a lot of assumptions further up the control stack
106 : // 1G decent probably plenty anyway....
107 20019 : limit.throttle_lower = true;
108 : }
109 49960 : if (throttle_thrust >= 1) {
110 0 : throttle_thrust = 1;
111 0 : limit.throttle_upper = true;
112 : }
113 :
114 : // rotate the thrust into bodyframe
115 49960 : Matrix3f rot;
116 49960 : Vector3f thrust_vec;
117 49960 : rot.from_euler312(_roll_offset, _pitch_offset, 0.0f);
118 :
119 :
120 : /*
121 : upwards thrust, independent of orientation
122 : */
123 49960 : thrust_vec.x = 0.0f;
124 49960 : thrust_vec.y = 0.0f;
125 49960 : thrust_vec.z = throttle_thrust;
126 49960 : thrust_vec = rot * thrust_vec;
127 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
128 1598720 : if (motor_enabled[i]) {
129 199832 : _thrust_rpyt_out[i] = thrust_vec.x * _forward_factor[i];
130 199832 : _thrust_rpyt_out[i] += thrust_vec.y * _right_factor[i];
131 199832 : _thrust_rpyt_out[i] += thrust_vec.z * _throttle_factor[i];
132 :
133 199832 : if (fabsf(_thrust_rpyt_out[i]) >= 1) {
134 : // if we hit this the mixer is probably scaled incorrectly
135 0 : limit.throttle_upper = true;
136 : }
137 199832 : _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i],-1.0f,1.0f);
138 : }
139 : }
140 :
141 :
142 : /*
143 : rotations: roll, pitch and yaw
144 : */
145 49960 : float rpy_ratio = 1.0f; // scale factor, output will be scaled by this ratio so it can all fit evenly
146 : float thrust[AP_MOTORS_MAX_NUM_MOTORS];
147 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
148 1598720 : if (motor_enabled[i]) {
149 199832 : thrust[i] = roll_thrust * _roll_factor[i];
150 199832 : thrust[i] += pitch_thrust * _pitch_factor[i];
151 199832 : thrust[i] += yaw_thrust * _yaw_factor[i];
152 199832 : float total_thrust = _thrust_rpyt_out[i] + thrust[i];
153 : // control input will be limited by motor range
154 199832 : if (total_thrust > 1.0f) {
155 37230 : rpy_ratio = MIN(rpy_ratio,(1.0f - _thrust_rpyt_out[i]) / thrust[i]);
156 162602 : } else if (total_thrust < -1.0f) {
157 11969 : rpy_ratio = MIN(rpy_ratio,(-1.0f -_thrust_rpyt_out[i]) / thrust[i]);
158 : }
159 : }
160 : }
161 :
162 : // set limit flags if output is being scaled
163 49960 : if (rpy_ratio < 1) {
164 29280 : limit.set_rpy(true);
165 : }
166 :
167 : // scale back rotations evenly so it will all fit
168 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
169 1598720 : if (motor_enabled[i]) {
170 199832 : _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i] + thrust[i] * rpy_ratio,-1.0f,1.0f);
171 : }
172 : }
173 :
174 : /*
175 : forward and lateral, independent of orentaiton
176 : */
177 49960 : thrust_vec.x = forward_thrust;
178 49960 : thrust_vec.y = right_thrust;
179 49960 : thrust_vec.z = 0.0f;
180 49960 : thrust_vec = rot * thrust_vec;
181 :
182 49960 : float horz_ratio = 1.0f;
183 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
184 1598720 : if (motor_enabled[i]) {
185 199832 : thrust[i] = thrust_vec.x * _forward_factor[i];
186 199832 : thrust[i] += thrust_vec.y * _right_factor[i];
187 199832 : thrust[i] += thrust_vec.z * _throttle_factor[i];
188 199832 : float total_thrust = _thrust_rpyt_out[i] + thrust[i];
189 : // control input will be limited by motor range
190 199832 : if (total_thrust > 1.0f) {
191 0 : horz_ratio = MIN(horz_ratio,(1.0f - _thrust_rpyt_out[i]) / thrust[i]);
192 199832 : } else if (total_thrust < -1.0f) {
193 0 : horz_ratio = MIN(horz_ratio,(-1.0f -_thrust_rpyt_out[i]) / thrust[i]);
194 : }
195 : }
196 : }
197 :
198 : // scale back evenly so it will all fit
199 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
200 1598720 : if (motor_enabled[i]) {
201 199832 : _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i] + thrust[i] * horz_ratio,-1.0f,1.0f);
202 : }
203 : }
204 :
205 : /*
206 : apply deadzone to revesible motors, this stops motors from reversing direction too often
207 : re-use yaw headroom param for deadzone, constain to a max of 25%
208 : */
209 49960 : const float deadzone = constrain_float(_yaw_headroom.get() * 0.001f,0.0f,0.25f);
210 1648680 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
211 1598720 : if (motor_enabled[i] && _reversible[i]) {
212 0 : if (is_negative(_thrust_rpyt_out[i])) {
213 0 : if ((_thrust_rpyt_out[i] > -deadzone) && is_positive(_last_thrust_out[i])) {
214 0 : _thrust_rpyt_out[i] = 0.0f;
215 : } else {
216 0 : _last_thrust_out[i] = _thrust_rpyt_out[i];
217 : }
218 0 : } else if (is_positive(_thrust_rpyt_out[i])) {
219 0 : if ((_thrust_rpyt_out[i] < deadzone) && is_negative(_last_thrust_out[i])) {
220 0 : _thrust_rpyt_out[i] = 0.0f;
221 : } else {
222 0 : _last_thrust_out[i] = _thrust_rpyt_out[i];
223 : }
224 : }
225 : }
226 : }
227 :
228 49960 : }
229 :
230 : // sets the roll and pitch offset, this rotates the thrust vector in body frame
231 : // these are typically set such that the throttle thrust vector is earth frame up
232 49958 : void AP_MotorsMatrix_6DoF_Scripting::set_roll_pitch(float roll_deg, float pitch_deg)
233 : {
234 49958 : _roll_offset = radians(roll_deg);
235 49958 : _pitch_offset = radians(pitch_deg);
236 49958 : }
237 :
238 : // add_motor, take roll, pitch, yaw, throttle(up), forward, right factors along with a bool if the motor is reversible and the testing order, called from scripting
239 4 : void AP_MotorsMatrix_6DoF_Scripting::add_motor(int8_t motor_num, float roll_factor, float pitch_factor, float yaw_factor, float throttle_factor, float forward_factor, float right_factor, bool reversible, uint8_t testing_order)
240 : {
241 4 : if (initialised_ok()) {
242 : // don't allow matrix to be changed after init
243 0 : return;
244 : }
245 :
246 : // ensure valid motor number is provided
247 4 : if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
248 4 : motor_enabled[motor_num] = true;
249 :
250 4 : _roll_factor[motor_num] = roll_factor;
251 4 : _pitch_factor[motor_num] = pitch_factor;
252 4 : _yaw_factor[motor_num] = yaw_factor;
253 :
254 4 : _throttle_factor[motor_num] = throttle_factor;
255 4 : _forward_factor[motor_num] = forward_factor;
256 4 : _right_factor[motor_num] = right_factor;
257 :
258 : // set order that motor appears in test
259 4 : _test_order[motor_num] = testing_order;
260 :
261 : // ensure valid motor number is provided
262 4 : SRV_Channel::Function function = SRV_Channels::get_motor_function(motor_num);
263 4 : SRV_Channels::set_aux_channel_default(function, motor_num);
264 :
265 : uint8_t chan;
266 4 : if (!SRV_Channels::find_channel(function, chan)) {
267 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
268 0 : return;
269 : }
270 :
271 4 : _reversible[motor_num] = reversible;
272 4 : if (_reversible[motor_num]) {
273 : // reversible, set to angle type hard code trim to 1500
274 0 : SRV_Channels::set_angle(function, 4500);
275 0 : SRV_Channels::set_trim_to_pwm_for(function, 1500);
276 : } else {
277 4 : SRV_Channels::set_range(function, 4500);
278 : }
279 4 : SRV_Channels::set_output_min_max(function, get_pwm_output_min(), get_pwm_output_max());
280 : }
281 : }
282 :
283 1 : bool AP_MotorsMatrix_6DoF_Scripting::init(uint8_t expected_num_motors) {
284 1 : uint8_t num_motors = 0;
285 33 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
286 32 : if (motor_enabled[i]) {
287 4 : num_motors++;
288 : }
289 : }
290 :
291 1 : set_initialised_ok(expected_num_motors == num_motors);
292 :
293 1 : if (!initialised_ok()) {
294 0 : _mav_type = MAV_TYPE_GENERIC;
295 0 : return false;
296 : }
297 :
298 1 : switch (num_motors) {
299 0 : case 3:
300 0 : _mav_type = MAV_TYPE_TRICOPTER;
301 0 : break;
302 1 : case 4:
303 1 : _mav_type = MAV_TYPE_QUADROTOR;
304 1 : break;
305 0 : case 6:
306 0 : _mav_type = MAV_TYPE_HEXAROTOR;
307 0 : break;
308 0 : case 8:
309 0 : _mav_type = MAV_TYPE_OCTOROTOR;
310 0 : break;
311 0 : case 10:
312 0 : _mav_type = MAV_TYPE_DECAROTOR;
313 0 : break;
314 0 : case 12:
315 0 : _mav_type = MAV_TYPE_DODECAROTOR;
316 0 : break;
317 0 : default:
318 0 : _mav_type = MAV_TYPE_GENERIC;
319 : }
320 :
321 1 : return true;
322 : }
323 :
324 : // singleton instance
325 : AP_MotorsMatrix_6DoF_Scripting *AP_MotorsMatrix_6DoF_Scripting::_singleton;
326 :
327 : #endif // AP_SCRIPTING_ENABLED
|
