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 : /*
17 : * AP_Motors6DOF.cpp - ArduSub motors library
18 : */
19 :
20 : #include <AP_BattMonitor/AP_BattMonitor.h>
21 : #include <AP_HAL/AP_HAL.h>
22 : #include "AP_Motors6DOF.h"
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : // parameters for the motor class
27 : const AP_Param::GroupInfo AP_Motors6DOF::var_info[] = {
28 : AP_NESTEDGROUPINFO(AP_MotorsMulticopter, 0),
29 : // @Param: 1_DIRECTION
30 : // @DisplayName: Motor normal or reverse
31 : // @Description: Used to change motor rotation directions without changing wires
32 : // @Values: 1:normal,-1:reverse
33 : // @User: Standard
34 : AP_GROUPINFO("1_DIRECTION", 1, AP_Motors6DOF, _motor_reverse[0], 1),
35 :
36 : // @Param: 2_DIRECTION
37 : // @DisplayName: Motor normal or reverse
38 : // @Description: Used to change motor rotation directions without changing wires
39 : // @Values: 1:normal,-1:reverse
40 : // @User: Standard
41 : AP_GROUPINFO("2_DIRECTION", 2, AP_Motors6DOF, _motor_reverse[1], 1),
42 :
43 : // @Param: 3_DIRECTION
44 : // @DisplayName: Motor normal or reverse
45 : // @Description: Used to change motor rotation directions without changing wires
46 : // @Values: 1:normal,-1:reverse
47 : // @User: Standard
48 : AP_GROUPINFO("3_DIRECTION", 3, AP_Motors6DOF, _motor_reverse[2], 1),
49 :
50 : // @Param: 4_DIRECTION
51 : // @DisplayName: Motor normal or reverse
52 : // @Description: Used to change motor rotation directions without changing wires
53 : // @Values: 1:normal,-1:reverse
54 : // @User: Standard
55 : AP_GROUPINFO("4_DIRECTION", 4, AP_Motors6DOF, _motor_reverse[3], 1),
56 :
57 : // @Param: 5_DIRECTION
58 : // @DisplayName: Motor normal or reverse
59 : // @Description: Used to change motor rotation directions without changing wires
60 : // @Values: 1:normal,-1:reverse
61 : // @User: Standard
62 : AP_GROUPINFO("5_DIRECTION", 5, AP_Motors6DOF, _motor_reverse[4], 1),
63 :
64 : // @Param: 6_DIRECTION
65 : // @DisplayName: Motor normal or reverse
66 : // @Description: Used to change motor rotation directions without changing wires
67 : // @Values: 1:normal,-1:reverse
68 : // @User: Standard
69 : AP_GROUPINFO("6_DIRECTION", 6, AP_Motors6DOF, _motor_reverse[5], 1),
70 :
71 : // @Param: 7_DIRECTION
72 : // @DisplayName: Motor normal or reverse
73 : // @Description: Used to change motor rotation directions without changing wires
74 : // @Values: 1:normal,-1:reverse
75 : // @User: Standard
76 : AP_GROUPINFO("7_DIRECTION", 7, AP_Motors6DOF, _motor_reverse[6], 1),
77 :
78 : // @Param: 8_DIRECTION
79 : // @DisplayName: Motor normal or reverse
80 : // @Description: Used to change motor rotation directions without changing wires
81 : // @Values: 1:normal,-1:reverse
82 : // @User: Standard
83 : AP_GROUPINFO("8_DIRECTION", 8, AP_Motors6DOF, _motor_reverse[7], 1),
84 :
85 : // @Param: FV_CPLNG_K
86 : // @DisplayName: Forward/vertical to pitch decoupling factor
87 : // @Description: (Vectored frame only) Used to decouple pitch from forward/vertical motion. 0 to disable, 1.2 normal
88 : // @Range: 0.0 1.5
89 : // @Increment: 0.1
90 : // @User: Standard
91 : AP_GROUPINFO("FV_CPLNG_K", 9, AP_Motors6DOF, _forwardVerticalCouplingFactor, 1.0),
92 :
93 : // @Param: 9_DIRECTION
94 : // @DisplayName: Motor normal or reverse
95 : // @Description: Used to change motor rotation directions without changing wires
96 : // @Values: 1:normal,-1:reverse
97 : // @User: Standard
98 : AP_GROUPINFO("9_DIRECTION", 10, AP_Motors6DOF, _motor_reverse[8], 1),
99 :
100 : // @Param: 10_DIRECTION
101 : // @DisplayName: Motor normal or reverse
102 : // @Description: Used to change motor rotation directions without changing wires
103 : // @Values: 1:normal,-1:reverse
104 : // @User: Standard
105 : AP_GROUPINFO("10_DIRECTION", 11, AP_Motors6DOF, _motor_reverse[9], 1),
106 :
107 : // @Param: 11_DIRECTION
108 : // @DisplayName: Motor normal or reverse
109 : // @Description: Used to change motor rotation directions without changing wires
110 : // @Values: 1:normal,-1:reverse
111 : // @User: Standard
112 : AP_GROUPINFO("11_DIRECTION", 12, AP_Motors6DOF, _motor_reverse[10], 1),
113 :
114 : // @Param: 12_DIRECTION
115 : // @DisplayName: Motor normal or reverse
116 : // @Description: Used to change motor rotation directions without changing wires
117 : // @Values: 1:normal,-1:reverse
118 : // @User: Standard
119 : AP_GROUPINFO("12_DIRECTION", 13, AP_Motors6DOF, _motor_reverse[11], 1),
120 :
121 : AP_GROUPEND
122 : };
123 :
124 83 : void AP_Motors6DOF::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
125 : {
126 : // remove existing motors
127 2739 : for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
128 2656 : remove_motor(i);
129 : }
130 :
131 : // hard coded config for supported frames
132 83 : switch ((sub_frame_t)frame_class) {
133 : // Motor # Roll Factor Pitch Factor Yaw Factor Throttle Factor Forward Factor Lateral Factor Testing Order
134 0 : case SUB_FRAME_BLUEROV1:
135 0 : _frame_class_string = "BLUEROV1";
136 0 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 1.0f, 0, 1);
137 0 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
138 0 : add_motor_raw_6dof(AP_MOTORS_MOT_3, -0.5f, 0.5f, 0, 0.45f, 0, 0, 3);
139 0 : add_motor_raw_6dof(AP_MOTORS_MOT_4, 0.5f, 0.5f, 0, 0.45f, 0, 0, 4);
140 0 : add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, -1.0f, 0, 1.0f, 0, 0, 5);
141 0 : add_motor_raw_6dof(AP_MOTORS_MOT_6, -0.25f, 0, 0, 0, 0, 1.0f, 6);
142 0 : break;
143 :
144 0 : case SUB_FRAME_VECTORED_6DOF_90DEG:
145 0 : _frame_class_string = "VECTORED_6DOF_90DEG";
146 0 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 1.0f, 1.0f, 0, 1.0f, 0, 0, 1);
147 0 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
148 0 : add_motor_raw_6dof(AP_MOTORS_MOT_3, 1.0f, -1.0f, 0, 1.0f, 0, 0, 3);
149 0 : add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 0, 0, 0, 1.0f, 4);
150 0 : add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, 0, 0, 0, 0, 1.0f, 5);
151 0 : add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, 1.0f, 0, 1.0f, 0, 0, 6);
152 0 : add_motor_raw_6dof(AP_MOTORS_MOT_7, 0, 0, -1.0f, 0, 1.0f, 0, 7);
153 0 : add_motor_raw_6dof(AP_MOTORS_MOT_8, -1.0f, -1.0f, 0, 1.0f, 0, 0, 8);
154 0 : break;
155 :
156 1 : case SUB_FRAME_VECTORED_6DOF:
157 1 : _frame_class_string = "VECTORED_6DOF";
158 1 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, 1.0f, 0, -1.0f, 1.0f, 1);
159 1 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, -1.0f, 2);
160 1 : add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 1.0f, 3);
161 1 : add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 1.0f, 0, 1.0f, -1.0f, 4);
162 1 : add_motor_raw_6dof(AP_MOTORS_MOT_5, 1.0f, -1.0f, 0, -1.0f, 0, 0, 5);
163 1 : add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, -1.0f, 0, -1.0f, 0, 0, 6);
164 1 : add_motor_raw_6dof(AP_MOTORS_MOT_7, 1.0f, 1.0f, 0, -1.0f, 0, 0, 7);
165 1 : add_motor_raw_6dof(AP_MOTORS_MOT_8, -1.0f, 1.0f, 0, -1.0f, 0, 0, 8);
166 1 : break;
167 :
168 82 : case SUB_FRAME_VECTORED:
169 82 : _frame_class_string = "VECTORED";
170 82 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, 1.0f, 0, -1.0f, 1.0f, 1);
171 82 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, -1.0f, 2);
172 82 : add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 1.0f, 3);
173 82 : add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 1.0f, 0, 1.0f, -1.0f, 4);
174 82 : add_motor_raw_6dof(AP_MOTORS_MOT_5, 1.0f, 0, 0, -1.0f, 0, 0, 5);
175 82 : add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, 0, 0, -1.0f, 0, 0, 6);
176 82 : break;
177 :
178 0 : case SUB_FRAME_CUSTOM:
179 : // Put your custom motor setup here
180 : //break;
181 :
182 : case SUB_FRAME_SIMPLEROV_3:
183 0 : _frame_class_string = "SIMPLEROV_3";
184 0 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 1.0f, 0, 1);
185 0 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
186 0 : add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, 0, -1.0f, 0, 0, 3);
187 0 : break;
188 0 : case SUB_FRAME_SIMPLEROV_4:
189 : case SUB_FRAME_SIMPLEROV_5:
190 : default:
191 0 : _frame_class_string = "DEFAULT";
192 0 : add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, -1.0f, 0, 1.0f, 0, 1);
193 0 : add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, 1.0f, 0, 1.0f, 0, 2);
194 0 : add_motor_raw_6dof(AP_MOTORS_MOT_3, 1.0f, 0, 0, -1.0f, 0, 0, 3);
195 0 : add_motor_raw_6dof(AP_MOTORS_MOT_4, -1.0f, 0, 0, -1.0f, 0, 0, 4);
196 0 : add_motor_raw_6dof(AP_MOTORS_MOT_5, 0, 0, 0, 0, 0, 1.0f, 5);
197 0 : break;
198 : }
199 83 : }
200 :
201 500 : void AP_Motors6DOF::add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac, uint8_t testing_order)
202 : {
203 : //Parent takes care of enabling output and setting up masks
204 500 : add_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order);
205 :
206 : //These are additional parameters for an ROV
207 500 : _throttle_factor[motor_num] = throttle_fac;
208 500 : _forward_factor[motor_num] = forward_fac;
209 500 : _lateral_factor[motor_num] = lat_fac;
210 500 : }
211 :
212 : // output_min - sends minimum values out to the motors
213 4851 : void AP_Motors6DOF::output_min()
214 : {
215 : int8_t i;
216 :
217 : // set limits flags
218 4851 : limit.set_rpy(true);
219 4851 : limit.set_throttle(false);
220 :
221 : // fill the motor_out[] array for HIL use and send minimum value to each motor
222 : // ToDo find a field to store the minimum pwm instead of hard coding 1500
223 160083 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
224 155232 : if (motor_enabled[i]) {
225 29110 : rc_write(i, 1500);
226 : }
227 : }
228 4851 : }
229 :
230 7387058 : int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
231 : {
232 7387058 : int16_t range_up = get_pwm_output_max() - 1500;
233 7387058 : int16_t range_down = 1500 - get_pwm_output_min();
234 7387058 : return 1500 + thrust_in * (thrust_in > 0 ? range_up : range_down);
235 : }
236 :
237 2180683 : void AP_Motors6DOF::output_to_motors()
238 : {
239 : int8_t i;
240 : int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
241 :
242 2180683 : switch (_spool_state) {
243 944244 : case SpoolState::SHUT_DOWN:
244 : // sends minimum values out to the motors
245 : // set motor output based on thrust requests
246 31160052 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
247 30215808 : if (motor_enabled[i]) {
248 5699784 : motor_out[i] = 1500;
249 : }
250 : }
251 944244 : break;
252 9115 : case SpoolState::GROUND_IDLE:
253 : // sends output to motors when armed but not flying
254 300795 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
255 291680 : if (motor_enabled[i]) {
256 55094 : motor_out[i] = 1500;
257 : }
258 : }
259 9115 : break;
260 1227324 : case SpoolState::SPOOLING_UP:
261 : case SpoolState::THROTTLE_UNLIMITED:
262 : case SpoolState::SPOOLING_DOWN:
263 : // set motor output based on thrust requests
264 40501692 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
265 39274368 : if (motor_enabled[i]) {
266 7387058 : motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
267 : }
268 : }
269 1227324 : break;
270 : }
271 :
272 : // send output to each motor
273 71962539 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
274 69781856 : if (motor_enabled[i]) {
275 13141936 : rc_write(i, motor_out[i]);
276 : }
277 : }
278 2180683 : }
279 :
280 1227322 : float AP_Motors6DOF::get_current_limit_max_throttle()
281 : {
282 1227322 : return 1.0f;
283 : }
284 :
285 : // output_armed - sends commands to the motors
286 : // includes new scaling stability patch
287 : // TODO pull code that is common to output_armed_not_stabilizing into helper functions
288 : // ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/lateral commands
289 2180683 : void AP_Motors6DOF::output_armed_stabilizing()
290 : {
291 2180683 : if ((sub_frame_t)_active_frame_class == SUB_FRAME_VECTORED) {
292 2151764 : output_armed_stabilizing_vectored();
293 28919 : } else if ((sub_frame_t)_active_frame_class == SUB_FRAME_VECTORED_6DOF) {
294 28919 : output_armed_stabilizing_vectored_6dof();
295 : } else {
296 : uint8_t i; // general purpose counter
297 : float roll_thrust; // roll thrust input value, +/- 1.0
298 : float pitch_thrust; // pitch thrust input value, +/- 1.0
299 : float yaw_thrust; // yaw thrust input value, +/- 1.0
300 : float throttle_thrust; // throttle thrust input value, +/- 1.0
301 : float forward_thrust; // forward thrust input value, +/- 1.0
302 : float lateral_thrust; // lateral thrust input value, +/- 1.0
303 :
304 0 : roll_thrust = (_roll_in + _roll_in_ff);
305 0 : pitch_thrust = (_pitch_in + _pitch_in_ff);
306 0 : yaw_thrust = (_yaw_in + _yaw_in_ff);
307 0 : throttle_thrust = get_throttle_bidirectional();
308 0 : forward_thrust = _forward_in;
309 0 : lateral_thrust = _lateral_in;
310 :
311 : float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
312 : float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
313 :
314 : // initialize limits flags
315 0 : limit.set_all(false);
316 :
317 : // sanity check throttle is above zero and below current limited throttle
318 0 : if (throttle_thrust <= -_throttle_thrust_max) {
319 0 : throttle_thrust = -_throttle_thrust_max;
320 0 : limit.throttle_lower = true;
321 : }
322 0 : if (throttle_thrust >= _throttle_thrust_max) {
323 0 : throttle_thrust = _throttle_thrust_max;
324 0 : limit.throttle_upper = true;
325 : }
326 :
327 : // calculate roll, pitch and yaw for each motor
328 0 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
329 0 : if (motor_enabled[i]) {
330 0 : rpy_out[i] = roll_thrust * _roll_factor[i] +
331 0 : pitch_thrust * _pitch_factor[i] +
332 0 : yaw_thrust * _yaw_factor[i];
333 :
334 : }
335 : }
336 :
337 : // calculate linear command for each motor
338 : // linear factors should be 0.0 or 1.0 for now
339 0 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
340 0 : if (motor_enabled[i]) {
341 0 : linear_out[i] = throttle_thrust * _throttle_factor[i] +
342 0 : forward_thrust * _forward_factor[i] +
343 0 : lateral_thrust * _lateral_factor[i];
344 : }
345 : }
346 :
347 : // Calculate final output for each motor
348 0 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
349 0 : if (motor_enabled[i]) {
350 0 : _thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]),-1.0f,1.0f);
351 : }
352 : }
353 : }
354 :
355 : #if AP_BATTERY_ENABLED
356 2180683 : const AP_BattMonitor &battery = AP::battery();
357 :
358 : // Current limiting
359 : float _batt_current;
360 2180683 : if (_batt_current_max <= 0.0f || !battery.current_amps(_batt_current)) {
361 2180683 : return;
362 : }
363 :
364 0 : float _batt_current_delta = _batt_current - _batt_current_last;
365 :
366 0 : float _current_change_rate = _batt_current_delta / _dt_s;
367 :
368 0 : float predicted_current = _batt_current + (_current_change_rate * _dt_s * 5);
369 :
370 0 : float batt_current_ratio = _batt_current / _batt_current_max;
371 :
372 0 : float predicted_current_ratio = predicted_current / _batt_current_max;
373 0 : _batt_current_last = _batt_current;
374 :
375 0 : if (predicted_current > _batt_current_max * 1.5f) {
376 0 : batt_current_ratio = 2.5f;
377 0 : } else if (_batt_current < _batt_current_max && predicted_current > _batt_current_max) {
378 0 : batt_current_ratio = predicted_current_ratio;
379 : }
380 0 : _output_limited += (_dt_s / (_dt_s + _batt_current_time_constant)) * (1 - batt_current_ratio);
381 : #endif
382 :
383 0 : _output_limited = constrain_float(_output_limited, 0.0f, 1.0f);
384 :
385 0 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
386 0 : if (motor_enabled[i]) {
387 0 : _thrust_rpyt_out[i] *= _output_limited;
388 : }
389 : }
390 : }
391 :
392 : // output_armed - sends commands to the motors
393 : // includes new scaling stability patch
394 : // TODO pull code that is common to output_armed_not_stabilizing into helper functions
395 : // ToDo calculate headroom for rpy to be added for stabilization during full throttle/forward/lateral commands
396 2151764 : void AP_Motors6DOF::output_armed_stabilizing_vectored()
397 : {
398 : uint8_t i; // general purpose counter
399 : float roll_thrust; // roll thrust input value, +/- 1.0
400 : float pitch_thrust; // pitch thrust input value, +/- 1.0
401 : float yaw_thrust; // yaw thrust input value, +/- 1.0
402 : float throttle_thrust; // throttle thrust input value, +/- 1.0
403 : float forward_thrust; // forward thrust input value, +/- 1.0
404 : float lateral_thrust; // lateral thrust input value, +/- 1.0
405 :
406 2151764 : roll_thrust = (_roll_in + _roll_in_ff);
407 2151764 : pitch_thrust = (_pitch_in + _pitch_in_ff);
408 2151764 : yaw_thrust = (_yaw_in + _yaw_in_ff);
409 2151764 : throttle_thrust = get_throttle_bidirectional();
410 2151764 : forward_thrust = _forward_in;
411 2151764 : lateral_thrust = _lateral_in;
412 :
413 : float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
414 : float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
415 :
416 : // initialize limits flags
417 2151764 : limit.set_all(false);
418 :
419 : // sanity check throttle is above zero and below current limited throttle
420 2151764 : if (throttle_thrust <= -_throttle_thrust_max) {
421 957826 : throttle_thrust = -_throttle_thrust_max;
422 957826 : limit.throttle_lower = true;
423 : }
424 :
425 2151764 : if (throttle_thrust >= _throttle_thrust_max) {
426 984495 : throttle_thrust = _throttle_thrust_max;
427 984495 : limit.throttle_upper = true;
428 : }
429 :
430 : // calculate roll, pitch and yaw for each motor
431 71008212 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
432 68856448 : if (motor_enabled[i]) {
433 12910584 : rpy_out[i] = roll_thrust * _roll_factor[i] +
434 12910584 : pitch_thrust * _pitch_factor[i] +
435 12910584 : yaw_thrust * _yaw_factor[i];
436 : }
437 : }
438 :
439 2151764 : float forward_coupling_limit = 1-_forwardVerticalCouplingFactor*float(fabsf(throttle_thrust));
440 2151764 : if (forward_coupling_limit < 0) {
441 0 : forward_coupling_limit = 0;
442 : }
443 2151764 : int8_t forward_coupling_direction[] = {-1,-1,1,1,0,0,0,0,0,0,0,0};
444 :
445 : // calculate linear command for each motor
446 : // linear factors should be 0.0 or 1.0 for now
447 71008212 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
448 68856448 : if (motor_enabled[i]) {
449 :
450 12910584 : float forward_thrust_limited = forward_thrust;
451 :
452 : // The following statements decouple forward/vertical hydrodynamic coupling on
453 : // vectored ROVs. This is done by limiting the maximum output of the "rear" vectored
454 : // thruster (where "rear" depends on direction of travel).
455 12910584 : if (!is_zero(forward_thrust_limited)) {
456 5418756 : if ((forward_thrust < 0) == (forward_coupling_direction[i] < 0) && forward_coupling_direction[i] != 0) {
457 1806252 : forward_thrust_limited = constrain_float(forward_thrust, -forward_coupling_limit, forward_coupling_limit);
458 : }
459 : }
460 :
461 12910584 : linear_out[i] = throttle_thrust * _throttle_factor[i] +
462 12910584 : forward_thrust_limited * _forward_factor[i] +
463 12910584 : lateral_thrust * _lateral_factor[i];
464 : }
465 : }
466 :
467 : // Calculate final output for each motor
468 71008212 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
469 68856448 : if (motor_enabled[i]) {
470 12910584 : _thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]), -1.0f, 1.0f);
471 : }
472 : }
473 2151764 : }
474 :
475 : // Band Aid fix for motor normalization issues.
476 : // TODO: find a global solution for managing saturation that works for all vehicles
477 28919 : void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
478 : {
479 : uint8_t i; // general purpose counter
480 : float roll_thrust; // roll thrust input value, +/- 1.0
481 : float pitch_thrust; // pitch thrust input value, +/- 1.0
482 : float yaw_thrust; // yaw thrust input value, +/- 1.0
483 : float throttle_thrust; // throttle thrust input value, +/- 1.0
484 : float forward_thrust; // forward thrust input value, +/- 1.0
485 : float lateral_thrust; // lateral thrust input value, +/- 1.0
486 :
487 28919 : roll_thrust = (_roll_in + _roll_in_ff);
488 28919 : pitch_thrust = (_pitch_in + _pitch_in_ff);
489 28919 : yaw_thrust = (_yaw_in + _yaw_in_ff);
490 28919 : throttle_thrust = get_throttle_bidirectional();
491 28919 : forward_thrust = _forward_in;
492 28919 : lateral_thrust = _lateral_in;
493 :
494 : float rpt_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
495 : float yfl_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
496 : float rpt_max;
497 : float yfl_max;
498 :
499 : // initialize limits flags
500 28919 : limit.set_all(false);
501 :
502 : // sanity check throttle is above zero and below current limited throttle
503 28919 : if (throttle_thrust <= -_throttle_thrust_max) {
504 17406 : throttle_thrust = -_throttle_thrust_max;
505 17406 : limit.throttle_lower = true;
506 : }
507 :
508 28919 : if (throttle_thrust >= _throttle_thrust_max) {
509 19102 : throttle_thrust = _throttle_thrust_max;
510 19102 : limit.throttle_upper = true;
511 : }
512 :
513 28919 : throttle_thrust = constrain_float(throttle_thrust, -1.0f, _max_throttle);
514 :
515 : // calculate roll, pitch and Throttle for each motor (only used by vertical thrusters)
516 28919 : rpt_max = 1; //Initialized to 1 so that normalization will only occur if value is saturated
517 954327 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
518 925408 : if (motor_enabled[i]) {
519 231352 : rpt_out[i] = roll_thrust * _roll_factor[i] +
520 231352 : pitch_thrust * _pitch_factor[i] +
521 231352 : throttle_thrust * _throttle_factor[i];
522 231352 : if (fabsf(rpt_out[i]) > rpt_max) {
523 5063 : rpt_max = fabsf(rpt_out[i]);
524 : }
525 : }
526 : }
527 :
528 : // calculate linear/yaw command for each motor (only used for translational thrusters)
529 : // linear factors should be 0.0 or 1.0 for now
530 28919 : yfl_max = 1; //Initialized to 1 so that normalization will only occur if value is saturated
531 954327 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
532 925408 : if (motor_enabled[i]) {
533 231352 : yfl_out[i] = yaw_thrust * _yaw_factor[i] +
534 231352 : forward_thrust * _forward_factor[i] +
535 231352 : lateral_thrust * _lateral_factor[i];
536 231352 : if (fabsf(yfl_out[i]) > yfl_max) {
537 0 : yfl_max = fabsf(yfl_out[i]);
538 : }
539 : }
540 : }
541 :
542 : // Calculate final output for each motor and normalize if necessary
543 954327 : for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
544 925408 : if (motor_enabled[i]) {
545 231352 : _thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpt_out[i]/rpt_max + yfl_out[i]/yfl_max),-1.0f,1.0f);
546 : }
547 : }
548 28919 : }
549 :
550 0 : Vector3f AP_Motors6DOF::get_motor_angular_factors(int motor_number) {
551 0 : if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
552 0 : return Vector3f(0,0,0);
553 : }
554 0 : return Vector3f(_roll_factor[motor_number], _pitch_factor[motor_number], _yaw_factor[motor_number]);
555 : }
556 :
557 0 : bool AP_Motors6DOF::motor_is_enabled(int motor_number) {
558 0 : if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
559 0 : return false;
560 : }
561 0 : return motor_enabled[motor_number];
562 : }
563 :
564 0 : bool AP_Motors6DOF::set_reversed(int motor_number, bool reversed) {
565 0 : if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
566 0 : return false;
567 : }
568 0 : if (reversed) {
569 0 : _motor_reverse[motor_number].set_and_save(-1);
570 : } else {
571 0 : _motor_reverse[motor_number].set_and_save(1);
572 : }
573 0 : return true;
574 : }
|
