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_Motors_config.h"
17 :
18 : #if AP_MOTORS_TRI_ENABLED
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_Vehicle/AP_Vehicle_Type.h>
22 :
23 : #include <AP_Math/AP_Math.h>
24 : #include <GCS_MAVLink/GCS.h>
25 : #include <SRV_Channel/SRV_Channel.h>
26 :
27 : #include "AP_MotorsTri.h"
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : // init
32 10 : void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_type)
33 : {
34 10 : add_motor_num(AP_MOTORS_MOT_1);
35 10 : add_motor_num(AP_MOTORS_MOT_2);
36 10 : add_motor_num(AP_MOTORS_MOT_4);
37 :
38 : // set update rate for the 3 motors (but not the servo on channel 7)
39 10 : set_update_rate(_speed_hz);
40 :
41 : // set the motor_enabled flag so that the ESCs can be calibrated like other frame types
42 10 : motor_enabled[AP_MOTORS_MOT_1] = true;
43 10 : motor_enabled[AP_MOTORS_MOT_2] = true;
44 10 : motor_enabled[AP_MOTORS_MOT_4] = true;
45 :
46 : // allow mapping of motor7
47 10 : add_motor_num(AP_MOTORS_CH_TRI_YAW);
48 :
49 : // Check for tail servo
50 10 : _have_tail_servo = SRV_Channels::function_assigned(SRV_Channel::k_motor7);
51 :
52 10 : SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
53 :
54 : // check for reverse tricopter
55 10 : _pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
56 :
57 10 : _mav_type = MAV_TYPE_TRICOPTER;
58 :
59 : // record successful initialisation if what we setup was the desired frame_class
60 10 : set_initialised_ok(frame_class == MOTOR_FRAME_TRI);
61 10 : }
62 :
63 : // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
64 42 : void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
65 : {
66 : // check for reverse tricopter
67 42 : _pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
68 :
69 42 : set_initialised_ok((frame_class == MOTOR_FRAME_TRI) && SRV_Channels::function_assigned(SRV_Channel::k_motor7));
70 42 : }
71 :
72 : // set update rate to motors - a value in hertz
73 20 : void AP_MotorsTri::set_update_rate(uint16_t speed_hz)
74 : {
75 : // record requested speed
76 20 : _speed_hz = speed_hz;
77 :
78 : // set update rate for the 3 motors (but not the servo on channel 7)
79 20 : uint32_t mask =
80 : 1U << AP_MOTORS_MOT_1 |
81 : 1U << AP_MOTORS_MOT_2 |
82 : 1U << AP_MOTORS_MOT_4;
83 20 : rc_set_freq(mask, _speed_hz);
84 20 : }
85 :
86 416691 : void AP_MotorsTri::output_to_motors()
87 : {
88 416691 : switch (_spool_state) {
89 215616 : case SpoolState::SHUT_DOWN:
90 : // sends minimum values out to the motors
91 7115328 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
92 6899712 : if (motor_enabled_mask(i)) {
93 464110 : _actuator[AP_MOTORS_MOT_1+i] = 0;
94 : }
95 : }
96 215616 : rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
97 215616 : break;
98 3018 : case SpoolState::GROUND_IDLE:
99 : // sends output to motors when armed but not flying
100 3018 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], actuator_spin_up_to_ground_idle());
101 3018 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], actuator_spin_up_to_ground_idle());
102 3018 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], actuator_spin_up_to_ground_idle());
103 3018 : rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
104 3018 : break;
105 198057 : case SpoolState::SPOOLING_UP:
106 : case SpoolState::THROTTLE_UNLIMITED:
107 : case SpoolState::SPOOLING_DOWN:
108 : // set motor output based on thrust requests
109 198057 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], thr_lin.thrust_to_actuator(_thrust_right));
110 198057 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], thr_lin.thrust_to_actuator(_thrust_left));
111 198057 : set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], thr_lin.thrust_to_actuator(_thrust_rear));
112 198057 : rc_write_angle(AP_MOTORS_CH_TRI_YAW, degrees(_pivot_angle)*100);
113 198057 : break;
114 : }
115 :
116 416691 : rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[AP_MOTORS_MOT_1]));
117 416691 : rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[AP_MOTORS_MOT_2]));
118 416691 : rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[AP_MOTORS_MOT_4]));
119 416691 : }
120 :
121 : // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
122 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
123 72 : uint32_t AP_MotorsTri::get_motor_mask()
124 : {
125 : // tri copter uses channels 1,2,4 and 7
126 72 : uint32_t motor_mask = (1U << AP_MOTORS_MOT_1) |
127 : (1U << AP_MOTORS_MOT_2) |
128 : (1U << AP_MOTORS_MOT_4);
129 72 : uint32_t mask = motor_mask_to_srv_channel_mask(motor_mask);
130 :
131 : // add parent's mask
132 72 : mask |= AP_MotorsMulticopter::get_motor_mask();
133 :
134 72 : return mask;
135 : }
136 :
137 : // output_armed - sends commands to the motors
138 : // includes new scaling stability patch
139 416691 : void AP_MotorsTri::output_armed_stabilizing()
140 : {
141 : float roll_thrust; // roll thrust input value, +/- 1.0
142 : float pitch_thrust; // pitch thrust input value, +/- 1.0
143 : float yaw_thrust; // yaw thrust input value, +/- 1.0
144 : float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
145 : float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
146 : float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
147 416691 : float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
148 416691 : float rpy_low = 0.0f; // lowest motor value
149 416691 : float rpy_high = 0.0f; // highest motor value
150 : float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
151 :
152 416691 : SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
153 :
154 : // sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
155 416691 : _yaw_servo_angle_max_deg.set(constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX));
156 :
157 : // apply voltage and air pressure compensation
158 416691 : const float compensation_gain = thr_lin.get_compensation_gain();
159 416691 : roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
160 416691 : pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
161 416691 : yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain * sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
162 416691 : throttle_thrust = get_throttle() * compensation_gain;
163 416691 : throttle_avg_max = _throttle_avg_max * compensation_gain;
164 :
165 : // check for reversed pitch
166 416691 : if (_pitch_reversed) {
167 0 : pitch_thrust *= -1.0f;
168 : }
169 :
170 : // VTOL plane may not have tail servo
171 416691 : if (!_have_tail_servo) {
172 0 : _pivot_angle = 0.0;
173 : } else {
174 : // calculate angle of yaw pivot
175 416691 : _pivot_angle = safe_asin(yaw_thrust);
176 416691 : if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
177 0 : limit.yaw = true;
178 0 : _pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
179 : }
180 : }
181 :
182 416691 : float pivot_thrust_max = cosf(_pivot_angle);
183 416691 : float thrust_max = 1.0f;
184 :
185 : // sanity check throttle is above zero and below current limited throttle
186 416691 : if (throttle_thrust <= 0.0f) {
187 231595 : throttle_thrust = 0.0f;
188 231595 : limit.throttle_lower = true;
189 : }
190 416691 : if (throttle_thrust >= _throttle_thrust_max) {
191 227231 : throttle_thrust = _throttle_thrust_max;
192 227231 : limit.throttle_upper = true;
193 : }
194 :
195 416691 : throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
196 :
197 : // The following mix may be offer less coupling between axis but needs testing
198 : //_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
199 : //_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
200 : //_thrust_rear = 0;
201 :
202 416691 : _thrust_right = roll_thrust * -0.5f + pitch_thrust * 0.5f;
203 416691 : _thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f;
204 416691 : _thrust_rear = pitch_thrust * -0.5f;
205 :
206 : // calculate roll and pitch for each motor
207 : // set rpy_low and rpy_high to the lowest and highest values of the motors
208 :
209 : // record lowest roll pitch command
210 416691 : rpy_low = MIN(_thrust_right, _thrust_left);
211 416691 : rpy_high = MAX(_thrust_right, _thrust_left);
212 416691 : if (rpy_low > _thrust_rear) {
213 119872 : rpy_low = _thrust_rear;
214 : }
215 : // check to see if the rear motor will reach maximum thrust before the front two motors
216 416691 : if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)) {
217 99467 : thrust_max = pivot_thrust_max;
218 99467 : rpy_high = _thrust_rear;
219 : }
220 :
221 : // calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
222 : // 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
223 : // 2. the higher of:
224 : // a) the pilot's throttle input
225 : // b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
226 : // Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
227 : // Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
228 : // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
229 : // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
230 :
231 : // check everything fits
232 416691 : throttle_thrust_best_rpy = MIN(0.5f * thrust_max - (rpy_low + rpy_high) / 2.0, throttle_avg_max);
233 416691 : if (is_zero(rpy_low)) {
234 200220 : rpy_scale = 1.0f;
235 : } else {
236 216471 : rpy_scale = constrain_float(-throttle_thrust_best_rpy / rpy_low, 0.0f, 1.0f);
237 : }
238 :
239 : // calculate how close the motors can come to the desired throttle
240 416691 : thr_adj = throttle_thrust - throttle_thrust_best_rpy;
241 416691 : if (rpy_scale < 1.0f) {
242 : // Full range is being used by roll, pitch, and yaw.
243 19045 : limit.roll = true;
244 19045 : limit.pitch = true;
245 19045 : if (thr_adj > 0.0f) {
246 0 : limit.throttle_upper = true;
247 : }
248 19045 : thr_adj = 0.0f;
249 : } else {
250 397646 : if (thr_adj < -(throttle_thrust_best_rpy + rpy_low)) {
251 : // Throttle can't be reduced to desired value
252 14026 : thr_adj = -(throttle_thrust_best_rpy + rpy_low);
253 383620 : } else if (thr_adj > thrust_max - (throttle_thrust_best_rpy + rpy_high)) {
254 : // Throttle can't be increased to desired value
255 10187 : thr_adj = thrust_max - (throttle_thrust_best_rpy + rpy_high);
256 10187 : limit.throttle_upper = true;
257 : }
258 : }
259 :
260 : // determine throttle thrust for harmonic notch
261 416691 : const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
262 : // compensation_gain can never be zero
263 416691 : _throttle_out = throttle_thrust_best_plus_adj / compensation_gain;
264 :
265 : // add scaled roll, pitch, constrained yaw and throttle for each motor
266 416691 : _thrust_right = throttle_thrust_best_plus_adj + rpy_scale * _thrust_right;
267 416691 : _thrust_left = throttle_thrust_best_plus_adj + rpy_scale * _thrust_left;
268 416691 : _thrust_rear = throttle_thrust_best_plus_adj + rpy_scale * _thrust_rear;
269 :
270 : // scale pivot thrust to account for pivot angle
271 : // we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
272 416691 : _thrust_rear = _thrust_rear / cosf(_pivot_angle);
273 :
274 : // constrain all outputs to 0.0f to 1.0f
275 : // test code should be run with these lines commented out as they should not do anything
276 416691 : _thrust_right = constrain_float(_thrust_right, 0.0f, 1.0f);
277 416691 : _thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
278 416691 : _thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
279 416691 : }
280 :
281 : // output_test_seq - spin a motor at the pwm value specified
282 : // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
283 : // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
284 0 : void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
285 : {
286 : // output to motors and servos
287 0 : if (!_pitch_reversed) {
288 0 : switch (motor_seq) {
289 0 : case 1:
290 : // front right motor
291 0 : rc_write(AP_MOTORS_MOT_1, pwm);
292 0 : break;
293 0 : case 2:
294 : // back motor
295 0 : rc_write(AP_MOTORS_MOT_4, pwm);
296 0 : break;
297 0 : case 3:
298 : // back servo
299 0 : rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
300 0 : break;
301 0 : case 4:
302 : // front left motor
303 0 : rc_write(AP_MOTORS_MOT_2, pwm);
304 0 : break;
305 0 : default:
306 : // do nothing
307 0 : break;
308 : }
309 : } else {
310 0 : switch (motor_seq) {
311 0 : case 1:
312 : // front motor
313 0 : rc_write(AP_MOTORS_MOT_4, pwm);
314 0 : break;
315 0 : case 2:
316 : // front servo
317 0 : rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
318 0 : break;
319 0 : case 3:
320 : // back right motor
321 0 : rc_write(AP_MOTORS_MOT_1, pwm);
322 0 : break;
323 0 : case 4:
324 : // back left motor
325 0 : rc_write(AP_MOTORS_MOT_2, pwm);
326 0 : break;
327 0 : default:
328 : // do nothing
329 0 : break;
330 : }
331 : }
332 0 : }
333 :
334 : /*
335 : call vehicle supplied thrust compensation if set. This allows for
336 : vehicle specific thrust compensation for motor arrangements such as
337 : the forward motors tilting
338 : */
339 416691 : void AP_MotorsTri::thrust_compensation(void)
340 : {
341 416691 : if (_thrust_compensation_callback) {
342 : // convert 3 thrust values into an array indexed by motor number
343 272408 : float thrust[4] { _thrust_right, _thrust_left, 0, _thrust_rear };
344 :
345 : // apply vehicle supplied compensation function
346 272408 : _thrust_compensation_callback(thrust, 4);
347 :
348 : // extract compensated thrust values
349 272408 : _thrust_right = thrust[0];
350 272408 : _thrust_left = thrust[1];
351 272408 : _thrust_rear = thrust[3];
352 : }
353 416691 : }
354 :
355 : /*
356 : override tricopter tail servo output in output_motor_mask
357 : */
358 111192 : void AP_MotorsTri::output_motor_mask(float thrust, uint32_t mask, float rudder_dt)
359 : {
360 : // normal multicopter output
361 111192 : AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt);
362 :
363 : // and override yaw servo
364 111192 : rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
365 111192 : }
366 :
367 481939 : float AP_MotorsTri::get_roll_factor(uint8_t i)
368 : {
369 481939 : float ret = 0.0f;
370 :
371 481939 : switch (i) {
372 : // right motor
373 249719 : case AP_MOTORS_MOT_1:
374 249719 : ret = -1.0f;
375 249719 : break;
376 : // left motor
377 232220 : case AP_MOTORS_MOT_2:
378 232220 : ret = 1.0f;
379 232220 : break;
380 : }
381 :
382 481939 : return ret;
383 : }
384 :
385 : // This function is currently only used by AP_Motors_test
386 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
387 : float AP_MotorsTri::get_pitch_factor_json(uint8_t i)
388 : {
389 : float ret = 0.0f;
390 :
391 : switch (i) {
392 : case AP_MOTORS_MOT_1: // front motors
393 : case AP_MOTORS_MOT_2:
394 : ret = 0.5f;
395 : break;
396 : case AP_MOTORS_MOT_4: // rear motor
397 : ret = -1.0f;
398 : break;
399 : }
400 :
401 : if (_pitch_reversed) {
402 : ret *= -1.0f;
403 : }
404 :
405 : return ret;
406 : }
407 : #endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
408 :
409 : // Run arming checks
410 537 : bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const
411 : {
412 : #if !APM_BUILD_TYPE(APM_BUILD_ArduPlane) // Tilt Rotors do not need a yaw servo
413 : // Check for yaw servo
414 43 : if (!_have_tail_servo) {
415 0 : hal.util->snprintf(buffer, buflen, "no SERVOx_FUNCTION set to Motor7 for tail servo");
416 0 : return false;
417 : }
418 : #endif
419 :
420 : // run base class checks
421 537 : return AP_MotorsMulticopter::arming_checks(buflen, buffer);
422 : }
423 :
424 : // This function is currently only used by AP_Motors_test
425 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
426 : // Get the testing order for the motors
427 : uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i)
428 : {
429 : if (!_pitch_reversed) {
430 : switch (i) {
431 : case AP_MOTORS_MOT_1: // front right motor
432 : return 1;
433 : case AP_MOTORS_MOT_4: // back motor
434 : return 2;
435 : case AP_MOTORS_CH_TRI_YAW: // back servo
436 : return 3;
437 : case AP_MOTORS_MOT_2: // front left motor
438 : return 4;
439 : default:
440 : return 0;
441 : }
442 : } else {
443 : switch (i) {
444 : case AP_MOTORS_MOT_4: // front motor
445 : return 1;
446 : case AP_MOTORS_CH_TRI_YAW: // front servo
447 : return 2;
448 : case AP_MOTORS_MOT_1: // back right motor
449 : return 3;
450 : case AP_MOTORS_MOT_2: // back left motor
451 : return 4;
452 : default:
453 : return 0;
454 : }
455 : }
456 : }
457 : #endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
458 :
459 : #endif // AP_MOTORS_TRI_ENABLED
|
