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 : This program is distributed in the hope that it will be useful,
7 : but WITHOUT ANY WARRANTY; without even the implied warranty of
8 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 : GNU General Public License for more details.
10 : You should have received a copy of the GNU General Public License
11 : along with this program. If not, see <http://www.gnu.org/licenses/>.
12 : */
13 :
14 : #include <AP_HAL/AP_HAL.h>
15 : #include <AP_Math/AP_Math.h>
16 : #include <SRV_Channel/SRV_Channel.h>
17 : #include <GCS_MAVLink/GCS.h>
18 : #include "AP_MotorsUGV.h"
19 : #include <AP_Relay/AP_Relay.h>
20 : #include <AP_BattMonitor/AP_BattMonitor.h>
21 : #include <AP_Scheduler/AP_Scheduler.h>
22 :
23 : #define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : // singleton instance
28 : AP_MotorsUGV *AP_MotorsUGV::_singleton;
29 :
30 : // parameters for the motor class
31 : const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = {
32 : // @Param: PWM_TYPE
33 : // @DisplayName: Motor Output PWM type
34 : // @Description: This selects the output PWM type as regular PWM, OneShot, Brushed motor support using PWM (duty cycle) with separated direction signal, Brushed motor support with separate throttle and direction PWM (duty cycle)
35 : // @Values: 0:Normal,1:OneShot,2:OneShot125,3:BrushedWithRelay,4:BrushedBiPolar,5:DShot150,6:DShot300,7:DShot600,8:DShot1200
36 : // @User: Advanced
37 : // @RebootRequired: True
38 : AP_GROUPINFO("PWM_TYPE", 1, AP_MotorsUGV, _pwm_type, PWMType::NORMAL),
39 :
40 : // @Param: PWM_FREQ
41 : // @DisplayName: Motor Output PWM freq for brushed motors
42 : // @Description: Motor Output PWM freq for brushed motors
43 : // @Units: kHz
44 : // @Range: 1 20
45 : // @Increment: 1
46 : // @User: Advanced
47 : // @RebootRequired: True
48 : AP_GROUPINFO("PWM_FREQ", 2, AP_MotorsUGV, _pwm_freq, 16),
49 :
50 : // @Param: SAFE_DISARM
51 : // @DisplayName: Motor PWM output disabled when disarmed
52 : // @Description: Disables motor PWM output when disarmed
53 : // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
54 : // @User: Advanced
55 : AP_GROUPINFO("SAFE_DISARM", 3, AP_MotorsUGV, _disarm_disable_pwm, 0),
56 :
57 : // @Param: THR_MIN
58 : // @DisplayName: Throttle minimum
59 : // @Description: Throttle minimum percentage the autopilot will apply. This is useful for handling a deadzone around low throttle and for preventing internal combustion motors cutting out during missions. Must be less than MOT_THR_MAX.
60 : // @Units: %
61 : // @Range: 0 20
62 : // @Increment: 1
63 : // @User: Standard
64 : AP_GROUPINFO("THR_MIN", 5, AP_MotorsUGV, _throttle_min, 0),
65 :
66 : // @Param: THR_MAX
67 : // @DisplayName: Throttle maximum
68 : // @Description: Throttle maximum percentage the autopilot will apply. This can be used to prevent overheating an ESC or motor on an electric rover
69 : // @Units: %
70 : // @Range: 5 100
71 : // @Increment: 1
72 : // @User: Standard
73 : AP_GROUPINFO("THR_MAX", 6, AP_MotorsUGV, _throttle_max, 100),
74 :
75 : // @Param: SLEWRATE
76 : // @DisplayName: Throttle slew rate
77 : // @Description: Throttle slew rate as a percentage of total range per second. A value of 100 allows the motor to change over its full range in one second. A value of zero disables the limit. Note some NiMH powered rovers require a lower setting of 40 to reduce current demand to avoid brownouts.
78 : // @Units: %/s
79 : // @Range: 0 1000
80 : // @Increment: 1
81 : // @User: Standard
82 : AP_GROUPINFO("SLEWRATE", 8, AP_MotorsUGV, _slew_rate, 100),
83 :
84 : // @Param: THST_EXPO
85 : // @DisplayName: Thrust Curve Expo
86 : // @Description: Thrust curve exponent (-1 to +1 with 0 being linear)
87 : // @Range: -1.0 1.0
88 : // @User: Advanced
89 : AP_GROUPINFO("THST_EXPO", 9, AP_MotorsUGV, _thrust_curve_expo, 0.0f),
90 :
91 : // 10 was VEC_THR_BASE
92 :
93 : // @Param: SPD_SCA_BASE
94 : // @DisplayName: Motor speed scaling base speed
95 : // @Description: Speed above which steering is scaled down when using regular steering/throttle vehicles. zero to disable speed scaling
96 : // @Units: m/s
97 : // @Range: 0 10
98 : // @User: Advanced
99 : AP_GROUPINFO("SPD_SCA_BASE", 11, AP_MotorsUGV, _speed_scale_base, 1.0f),
100 :
101 : // @Param: STR_THR_MIX
102 : // @DisplayName: Motor steering vs throttle prioritisation
103 : // @Description: Steering vs Throttle priorisation. Higher numbers prioritise steering, lower numbers prioritise throttle. Only valid for Skid Steering vehicles
104 : // @Range: 0.2 1.0
105 : // @User: Advanced
106 : AP_GROUPINFO("STR_THR_MIX", 12, AP_MotorsUGV, _steering_throttle_mix, 0.5f),
107 :
108 : // @Param: VEC_ANGLEMAX
109 : // @DisplayName: Vector thrust angle max
110 : // @Description: The angle between steering's middle position and maximum position when using vectored thrust (boats only)
111 : // @Units: deg
112 : // @Range: 0 90
113 : // @User: Standard
114 : AP_GROUPINFO("VEC_ANGLEMAX", 13, AP_MotorsUGV, _vector_angle_max, 0.0f),
115 :
116 : // @Param: THST_ASYM
117 : // @DisplayName: Motor Thrust Asymmetry
118 : // @Description: Thrust Asymmetry. Used for skid-steering. 2.0 means your motors move twice as fast forward than they do backwards.
119 : // @Range: 1.0 10.0
120 : // @User: Advanced
121 : AP_GROUPINFO("THST_ASYM", 14, AP_MotorsUGV, _thrust_asymmetry, 1.0f),
122 :
123 : // @Param: REV_DELAY
124 : // @DisplayName: Motor reversal delay
125 : // @Description: For reversible motors that need a delay before they can change direction. When greater than zero the throttle will go to zero for this amount of time before outputting the new throttle when the demanded motor direction changes.
126 : // @Units: s
127 : // @Range: 0.1 1.0
128 : // @Increment: 0.1
129 : // @User: Standard
130 : AP_GROUPINFO("REV_DELAY", 15, AP_MotorsUGV, _reverse_delay, 0),
131 :
132 : #if AP_BATTERY_WATT_MAX_ENABLED
133 : // @Param: BAT_WATT_TC
134 : // @DisplayName: Battery power limiting time constant
135 : // @Description: Time constant used to limit the smooth power throttling.
136 : // @Range: 0 10
137 : // @Units: s
138 : // @User: Advanced
139 : AP_GROUPINFO("BAT_WATT_TC", 16, AP_MotorsUGV, _batt_power_time_constant, 2.0f),
140 : #endif
141 :
142 : AP_GROUPEND
143 : };
144 :
145 285 : AP_MotorsUGV::AP_MotorsUGV(AP_WheelRateControl& rate_controller) :
146 285 : _rate_controller(rate_controller)
147 : {
148 285 : AP_Param::setup_object_defaults(this, var_info);
149 285 : _singleton = this;
150 285 : }
151 :
152 283 : void AP_MotorsUGV::init(uint8_t frtype)
153 : {
154 283 : _frame_type = frame_type(frtype);
155 :
156 : // setup for omni vehicles
157 283 : if (_frame_type != FRAME_TYPE_UNDEFINED) {
158 1 : setup_omni();
159 : }
160 :
161 : // setup servo output
162 283 : setup_servo_output();
163 :
164 : // setup pwm type
165 283 : setup_pwm_type();
166 :
167 : // set safety output
168 283 : setup_safety_output();
169 :
170 283 : }
171 :
172 283 : bool AP_MotorsUGV::get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const
173 : {
174 283 : if (_pwm_type != PWMType::BRUSHED_WITH_RELAY) {
175 : // Relays only used if PWM type is set to brushed with relay
176 283 : return false;
177 : }
178 :
179 : // First relay is always used, throttle, throttle left or motor 1
180 0 : index1 = 0;
181 :
182 : // Second relay is used for right throttle on skid steer and motor 2 for omni
183 0 : if (have_skid_steering()) {
184 0 : index2 = 1;
185 : }
186 :
187 : // Omni can have a variable number of motors
188 0 : if (is_omni()) {
189 : // Omni has at least 3 motors
190 0 : index2 = 2;
191 0 : if (_motors_num >= 4) {
192 0 : index2 = 3;
193 : }
194 : }
195 :
196 0 : return true;
197 : }
198 :
199 : // setup output in case of main CPU failure
200 8082 : void AP_MotorsUGV::setup_safety_output()
201 : {
202 8082 : if (_pwm_type == PWMType::BRUSHED_WITH_RELAY) {
203 : // set trim to min to set duty cycle range (0 - 100%) to servo range
204 : // ignore servo revese flag, it is used by the relay
205 0 : SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle, true);
206 0 : SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleLeft, true);
207 0 : SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleRight, true);
208 : }
209 :
210 : // stop sending pwm if main CPU fails
211 8082 : SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
212 8082 : SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
213 8082 : SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
214 8082 : }
215 :
216 : // setup servo output ranges
217 8082 : void AP_MotorsUGV::setup_servo_output()
218 : {
219 : // k_steering are limited to -45;45 degree
220 8082 : SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
221 :
222 : // k_throttle are in power percent so -100 ... 100
223 8082 : SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
224 :
225 : // skid steering left/right throttle as -1000 to 1000 values
226 8082 : SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000);
227 8082 : SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
228 :
229 : // omni motors set in power percent so -100 ... 100
230 40410 : for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
231 32328 : SRV_Channel::Function function = SRV_Channels::get_motor_function(i);
232 32328 : SRV_Channels::set_angle(function, 100);
233 : }
234 :
235 : // mainsail range from 0 to 100
236 8082 : SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100);
237 : // wing sail -100 to 100
238 8082 : SRV_Channels::set_angle(SRV_Channel::k_wingsail_elevator, 100);
239 : // mast rotation -100 to 100
240 8082 : SRV_Channels::set_angle(SRV_Channel::k_mast_rotation, 100);
241 :
242 8082 : }
243 :
244 : // set steering as a value from -4500 to +4500
245 : // apply_scaling should be set to false for manual modes where
246 : // no scaling by speed or angle should be performed
247 629044 : void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
248 : {
249 629044 : _steering = steering;
250 629044 : _scale_steering = apply_scaling;
251 629044 : }
252 :
253 : // set throttle as a value from -100 to 100
254 629044 : void AP_MotorsUGV::set_throttle(float throttle)
255 : {
256 : // only allow setting throttle if armed
257 629044 : if (!hal.util->get_soft_armed()) {
258 378000 : return;
259 : }
260 :
261 : // check throttle is between -_throttle_max and +_throttle_max
262 251044 : _throttle = constrain_float(throttle, -_throttle_max, _throttle_max);
263 : }
264 :
265 : // set lateral input as a value from -100 to +100
266 362216 : void AP_MotorsUGV::set_lateral(float lateral)
267 : {
268 362216 : _lateral = constrain_float(lateral, -100.0f, 100.0f);
269 362216 : }
270 :
271 : // set roll input as a value from -1 to +1
272 362133 : void AP_MotorsUGV::set_roll(float roll)
273 : {
274 362133 : _roll = constrain_float(roll, -1.0f, 1.0f);
275 362133 : }
276 :
277 : // set pitch input as a value from -1 to +1
278 362133 : void AP_MotorsUGV::set_pitch(float pitch)
279 : {
280 362133 : _pitch = constrain_float(pitch, -1.0f, 1.0f);
281 362133 : }
282 :
283 : // set walking_height input as a value from -1 to +1
284 362133 : void AP_MotorsUGV::set_walking_height(float walking_height)
285 : {
286 362133 : _walking_height = constrain_float(walking_height, -1.0f, 1.0f);
287 362133 : }
288 :
289 : // set mainsail input as a value from 0 to 100
290 426905 : void AP_MotorsUGV::set_mainsail(float mainsail)
291 : {
292 426905 : _mainsail = constrain_float(mainsail, 0.0f, 100.0f);
293 426905 : }
294 :
295 : // set wingsail input as a value from -100 to 100
296 426905 : void AP_MotorsUGV::set_wingsail(float wingsail)
297 : {
298 426905 : _wingsail = constrain_float(wingsail, -100.0f, 100.0f);
299 426905 : }
300 :
301 : // set mast rotation input as a value from -100 to 100
302 426905 : void AP_MotorsUGV::set_mast_rotation(float mast_rotation)
303 : {
304 426905 : _mast_rotation = constrain_float(mast_rotation, -100.0f, 100.0f);
305 426905 : }
306 :
307 : // get slew limited throttle
308 : // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
309 : // same as private slew_limit_throttle method (see below) but does not update throttle state
310 991116 : float AP_MotorsUGV::get_slew_limited_throttle(float throttle, float dt) const
311 : {
312 991116 : if (_slew_rate <= 0) {
313 116970 : return throttle;
314 : }
315 :
316 874146 : const float throttle_change_max = static_cast<float>(_slew_rate) * dt;
317 874146 : return constrain_float(throttle, _throttle_prev - throttle_change_max, _throttle_prev + throttle_change_max);
318 : }
319 :
320 : /*
321 : work out if skid steering is available
322 : */
323 1099408 : bool AP_MotorsUGV::have_skid_steering() const
324 : {
325 1099408 : return (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) || is_omni();
326 : }
327 :
328 : // true if the vehicle has a mainsail
329 644109 : bool AP_MotorsUGV::has_sail() const
330 : {
331 644109 : return SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet) || SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator) || SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation);
332 : }
333 :
334 628798 : void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
335 : {
336 : // soft-armed overrides passed in armed status
337 628798 : if (!hal.util->get_soft_armed()) {
338 378000 : armed = false;
339 378000 : _throttle = 0.0f;
340 : }
341 :
342 : // clear limit flags
343 : // output_ methods are responsible for setting them to true if required on each iteration
344 628798 : limit.steer_left = limit.steer_right = limit.throttle_lower = limit.throttle_upper = false;
345 :
346 : // sanity check parameters
347 628798 : sanity_check_parameters();
348 :
349 : // slew limit throttle
350 628798 : slew_limit_throttle(dt);
351 :
352 : // apply power limiting to throttle
353 : #if AP_BATTERY_WATT_MAX_ENABLED
354 628798 : power_limit_throttle(dt);
355 : #endif
356 :
357 : // output for regular steering/throttle style frames
358 628798 : output_regular(armed, ground_speed, _steering, _throttle);
359 :
360 : // output for skid steering style frames
361 628798 : output_skid_steering(armed, _steering, _throttle, dt);
362 :
363 : // output for omni frames
364 628798 : output_omni(armed, _steering, _throttle, _lateral);
365 :
366 : // output to sails
367 628798 : output_sail();
368 :
369 : // send values to the PWM timers for output
370 628798 : auto &srv = AP::srv();
371 628798 : SRV_Channels::calc_pwm();
372 628798 : srv.cork();
373 628798 : SRV_Channels::output_ch_all();
374 628798 : srv.push();
375 628798 : }
376 :
377 : // test steering or throttle output as a percentage of the total (range -100 to +100)
378 : // used in response to DO_MOTOR_TEST mavlink command
379 0 : bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
380 : {
381 : // check if the motor_seq is valid
382 0 : if (motor_seq >= MOTOR_TEST_LAST) {
383 0 : return false;
384 : }
385 0 : pct = constrain_float(pct, -100.0f, 100.0f);
386 :
387 0 : switch (motor_seq) {
388 0 : case MOTOR_TEST_THROTTLE: {
389 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor1)) {
390 0 : output_throttle(SRV_Channel::k_motor1, pct);
391 : }
392 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
393 0 : output_throttle(SRV_Channel::k_throttle, pct);
394 : }
395 0 : break;
396 : }
397 0 : case MOTOR_TEST_STEERING: {
398 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) {
399 0 : output_throttle(SRV_Channel::k_motor2, pct);
400 : }
401 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
402 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, pct * 45.0f);
403 : }
404 0 : break;
405 : }
406 0 : case MOTOR_TEST_THROTTLE_LEFT: {
407 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) {
408 0 : output_throttle(SRV_Channel::k_motor3, pct);
409 : }
410 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
411 0 : output_throttle(SRV_Channel::k_throttleLeft, pct);
412 : }
413 0 : break;
414 : }
415 0 : case MOTOR_TEST_THROTTLE_RIGHT: {
416 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor4)) {
417 0 : output_throttle(SRV_Channel::k_motor4, pct);
418 : }
419 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
420 0 : output_throttle(SRV_Channel::k_throttleRight, pct);
421 : }
422 0 : break;
423 : }
424 0 : case MOTOR_TEST_MAINSAIL: {
425 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) {
426 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, pct);
427 : }
428 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
429 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, pct);
430 : }
431 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) {
432 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, pct);
433 : }
434 0 : break;
435 : }
436 0 : case MOTOR_TEST_LAST:
437 0 : return false;
438 : }
439 0 : auto &srv = AP::srv();
440 0 : SRV_Channels::calc_pwm();
441 0 : srv.cork();
442 0 : SRV_Channels::output_ch_all();
443 0 : srv.push();
444 0 : return true;
445 : }
446 :
447 : // test steering or throttle output using a pwm value
448 249 : bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm)
449 : {
450 : // check if the motor_seq is valid
451 249 : if (motor_seq > MOTOR_TEST_THROTTLE_RIGHT) {
452 0 : return false;
453 : }
454 249 : switch (motor_seq) {
455 249 : case MOTOR_TEST_THROTTLE: {
456 249 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor1)) {
457 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_motor1, pwm);
458 : }
459 249 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
460 249 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, pwm);
461 : }
462 249 : break;
463 : }
464 0 : case MOTOR_TEST_STEERING: {
465 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) {
466 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_motor2, pwm);
467 : }
468 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
469 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_steering, pwm);
470 : }
471 0 : break;
472 : }
473 0 : case MOTOR_TEST_THROTTLE_LEFT: {
474 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) {
475 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_motor3, pwm);
476 : }
477 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
478 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm);
479 : }
480 0 : break;
481 : }
482 0 : case MOTOR_TEST_THROTTLE_RIGHT: {
483 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_motor4)) {
484 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_motor4, pwm);
485 : }
486 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
487 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm);
488 : }
489 0 : break;
490 : }
491 0 : case MOTOR_TEST_MAINSAIL: {
492 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) {
493 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_mainsail_sheet, pwm);
494 : }
495 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_wingsail_elevator)) {
496 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_wingsail_elevator, pwm);
497 : }
498 0 : if (SRV_Channels::function_assigned(SRV_Channel::k_mast_rotation)) {
499 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_mast_rotation, pwm);
500 : }
501 0 : break;
502 : }
503 0 : default:
504 0 : return false;
505 : }
506 249 : auto &srv = AP::srv();
507 249 : SRV_Channels::calc_pwm();
508 249 : srv.cork();
509 249 : SRV_Channels::output_ch_all();
510 249 : srv.push();
511 249 : return true;
512 : }
513 :
514 : // returns true if checks pass, false if they fail. report should be true to send text messages to GCS
515 15226 : bool AP_MotorsUGV::pre_arm_check(bool report) const
516 : {
517 15226 : const bool have_throttle = SRV_Channels::function_assigned(SRV_Channel::k_throttle);
518 15226 : const bool have_throttle_left = SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft);
519 15226 : const bool have_throttle_right = SRV_Channels::function_assigned(SRV_Channel::k_throttleRight);
520 :
521 : // check that there's defined outputs, inc scripting and sail
522 44113 : if(!have_throttle_left &&
523 13661 : !have_throttle_right &&
524 13661 : !have_throttle &&
525 85 : !SRV_Channels::function_assigned(SRV_Channel::k_steering) &&
526 85 : !SRV_Channels::function_assigned(SRV_Channel::k_scripting1) &&
527 28972 : !has_sail() &&
528 85 : !is_omni()) {
529 0 : if (report) {
530 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: no motor, sail or scripting outputs defined");
531 : }
532 0 : return false;
533 : }
534 : // check if only one of skid-steering output has been configured
535 15226 : if (have_throttle_left != have_throttle_right) {
536 0 : if (report) {
537 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check skid steering config");
538 : }
539 0 : return false;
540 : }
541 : // check if only one of throttle or steering outputs has been configured, if has a sail allow no throttle
542 15226 : if ((has_sail() || have_throttle) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
543 0 : if (report) {
544 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check steering and throttle config");
545 : }
546 0 : return false;
547 : }
548 : // check all omni motor outputs have been configured
549 15481 : for (uint8_t i=0; i<_motors_num; i++) {
550 255 : SRV_Channel::Function function = SRV_Channels::get_motor_function(i);
551 255 : if (!SRV_Channels::function_assigned(function)) {
552 0 : if (report) {
553 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: servo function %u unassigned", function);
554 : }
555 0 : return false;
556 : }
557 : }
558 :
559 : // Check relays are configured for brushed with relay outputs
560 : #if AP_RELAY_ENABLED
561 15226 : AP_Relay*relay = AP::relay();
562 15226 : if ((_pwm_type == PWMType::BRUSHED_WITH_RELAY) && (relay != nullptr)) {
563 : // If a output is configured its relay must be enabled
564 : struct RelayTable {
565 : bool output_assigned;
566 : AP_Relay_Params::FUNCTION fun;
567 : };
568 :
569 0 : const RelayTable relay_table[] = {
570 0 : { have_throttle || have_throttle_left || (SRV_Channels::function_assigned(SRV_Channel::k_motor1) && (_motors_num >= 1)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1 },
571 0 : { have_throttle_right || (SRV_Channels::function_assigned(SRV_Channel::k_motor2) && (_motors_num >= 2)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2 },
572 0 : { SRV_Channels::function_assigned(SRV_Channel::k_motor3) && (_motors_num >= 3), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3 },
573 0 : { SRV_Channels::function_assigned(SRV_Channel::k_motor4) && (_motors_num >= 4), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4 },
574 0 : };
575 :
576 0 : for (uint8_t i=0; i<ARRAY_SIZE(relay_table); i++) {
577 0 : if (relay_table[i].output_assigned && !relay->enabled(relay_table[i].fun)) {
578 0 : if (report) {
579 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: relay function %u unassigned", uint8_t(relay_table[i].fun));
580 : }
581 0 : return false;
582 : }
583 : }
584 : }
585 : #endif
586 :
587 15226 : return true;
588 : }
589 :
590 : // sanity check parameters
591 628798 : void AP_MotorsUGV::sanity_check_parameters()
592 : {
593 628798 : _throttle_max.set(constrain_int16(_throttle_max, 5, 100));
594 628798 : _throttle_min.set(constrain_int16(_throttle_min, 0, MIN(20, _throttle_max)));
595 628798 : _vector_angle_max.set(constrain_float(_vector_angle_max, 0.0f, 90.0f));
596 628798 : }
597 :
598 : // setup pwm output type
599 283 : void AP_MotorsUGV::setup_pwm_type()
600 : {
601 283 : _motor_mask = 0;
602 :
603 283 : hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
604 :
605 : // work out mask of channels assigned to motors
606 283 : _motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttle);
607 283 : _motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleLeft);
608 283 : _motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleRight);
609 286 : for (uint8_t i=0; i<_motors_num; i++) {
610 3 : _motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channels::get_motor_function(i));
611 : }
612 :
613 283 : switch (_pwm_type) {
614 0 : case PWMType::ONESHOT:
615 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
616 0 : break;
617 0 : case PWMType::ONESHOT125:
618 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT125);
619 0 : break;
620 0 : case PWMType::BRUSHED_WITH_RELAY:
621 : case PWMType::BRUSHED_BIPOLAR:
622 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
623 0 : hal.rcout->set_freq(_motor_mask, uint16_t(_pwm_freq * 1000));
624 0 : break;
625 0 : case PWMType::DSHOT150:
626 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_DSHOT150);
627 0 : break;
628 0 : case PWMType::DSHOT300:
629 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_DSHOT300);
630 0 : break;
631 0 : case PWMType::DSHOT600:
632 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_DSHOT600);
633 0 : break;
634 0 : case PWMType::DSHOT1200:
635 0 : hal.rcout->set_output_mode(_motor_mask, AP_HAL::RCOutput::MODE_PWM_DSHOT1200);
636 0 : break;
637 283 : default:
638 : // do nothing
639 283 : break;
640 : }
641 283 : }
642 :
643 : // setup for frames with omni motors
644 1 : void AP_MotorsUGV::setup_omni()
645 : {
646 : // remove existing motors
647 5 : for (int8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
648 4 : clear_omni_motors(i);
649 : }
650 :
651 : // hard coded factor configuration
652 1 : switch (_frame_type) {
653 :
654 : // FRAME TYPE NAME
655 0 : case FRAME_TYPE_UNDEFINED:
656 0 : break;
657 :
658 0 : case FRAME_TYPE_OMNI3:
659 0 : _motors_num = 3;
660 0 : add_omni_motor(0, 1.0f, -1.0f, -1.0f);
661 0 : add_omni_motor(1, 0.0f, -1.0f, 1.0f);
662 0 : add_omni_motor(2, 1.0f, 1.0f, 1.0f);
663 0 : break;
664 :
665 0 : case FRAME_TYPE_OMNIX:
666 0 : _motors_num = 4,
667 0 : add_omni_motor(0, 1.0f, -1.0f, -1.0f);
668 0 : add_omni_motor(1, 1.0f, -1.0f, 1.0f);
669 0 : add_omni_motor(2, 1.0f, 1.0f, -1.0f);
670 0 : add_omni_motor(3, 1.0f, 1.0f, 1.0f);
671 0 : break;
672 :
673 0 : case FRAME_TYPE_OMNIPLUS:
674 0 : _motors_num = 4;
675 0 : add_omni_motor(0, 0.0f, 1.0f, 1.0f);
676 0 : add_omni_motor(1, 1.0f, 0.0f, 0.0f);
677 0 : add_omni_motor(2, 0.0f, -1.0f, 1.0f);
678 0 : add_omni_motor(3, 1.0f, 0.0f, 0.0f);
679 0 : break;
680 :
681 1 : case FRAME_TYPE_OMNI3MECANUM:
682 1 : _motors_num = 3;
683 1 : add_omni_motor(0, -1.0f, 1.0f, -0.26795f);
684 1 : add_omni_motor(1, 0.73205f, 1.0f, -0.73205f);
685 1 : add_omni_motor(2, 0.26795f, 1.0f, 1.0f);
686 1 : break;
687 : }
688 1 : }
689 :
690 : // add omni motor using separate throttle, steering and lateral factors
691 3 : void AP_MotorsUGV::add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor)
692 : {
693 : // ensure valid motor number is provided
694 3 : if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
695 :
696 : // set throttle, steering and lateral factors
697 3 : _throttle_factor[motor_num] = throttle_factor;
698 3 : _steering_factor[motor_num] = steering_factor;
699 3 : _lateral_factor[motor_num] = lateral_factor;
700 :
701 3 : add_omni_motor_num(motor_num);
702 : }
703 3 : }
704 :
705 : // add an omni motor and set up default output function
706 3 : void AP_MotorsUGV::add_omni_motor_num(int8_t motor_num)
707 : {
708 : // ensure a valid motor number is provided
709 3 : if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
710 : uint8_t chan;
711 3 : SRV_Channel::Function function = SRV_Channels::get_motor_function(motor_num);
712 3 : SRV_Channels::set_aux_channel_default(function, motor_num);
713 3 : if (!SRV_Channels::find_channel(function, chan)) {
714 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
715 : }
716 : }
717 3 : }
718 :
719 : // disable omni motor and remove all throttle, steering and lateral factor for this motor
720 4 : void AP_MotorsUGV::clear_omni_motors(int8_t motor_num)
721 : {
722 : // ensure valid motor number is provided
723 4 : if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
724 : // disable the motor and set factors to zero
725 4 : _throttle_factor[motor_num] = 0;
726 4 : _steering_factor[motor_num] = 0;
727 4 : _lateral_factor[motor_num] = 0;
728 : }
729 4 : }
730 :
731 : // output to regular steering and throttle channels
732 628798 : void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
733 : {
734 : // output to throttle channels
735 628798 : if (armed) {
736 250798 : if (_scale_steering) {
737 : // vectored thrust handling
738 213545 : if (have_vectored_thrust()) {
739 :
740 : // normalise desired steering and throttle to ease calculations
741 2485 : float steering_norm = steering / 4500.0f;
742 2485 : const float throttle_norm = throttle * 0.01f;
743 :
744 : // steering can never be more than throttle * tan(_vector_angle_max)
745 2485 : const float vector_angle_max_rad = radians(constrain_float(_vector_angle_max, 0.0f, 90.0f));
746 2485 : const float steering_norm_lim = fabsf(throttle_norm * tanf(vector_angle_max_rad));
747 2485 : if (fabsf(steering_norm) > steering_norm_lim) {
748 0 : if (is_positive(steering_norm)) {
749 0 : steering_norm = steering_norm_lim;
750 : }
751 0 : if (is_negative(steering_norm)) {
752 0 : steering_norm = -steering_norm_lim;
753 : }
754 0 : limit.steer_right = true;
755 0 : limit.steer_left = true;
756 : }
757 :
758 2485 : if (!is_zero(throttle_norm)) {
759 : // calculate steering angle
760 2485 : float steering_angle_rad = atanf(steering_norm / throttle_norm);
761 : // limit steering angle to vector_angle_max
762 2485 : if (fabsf(steering_angle_rad) > vector_angle_max_rad) {
763 0 : steering_angle_rad = constrain_float(steering_angle_rad, -vector_angle_max_rad, vector_angle_max_rad);
764 0 : limit.steer_right = true;
765 0 : limit.steer_left = true;
766 : }
767 :
768 : // convert steering angle to steering output
769 2485 : steering = steering_angle_rad / vector_angle_max_rad * 4500.0f;
770 :
771 : // scale up throttle to compensate for steering angle
772 2485 : const float throttle_scaler_inv = cosf(steering_angle_rad);
773 2485 : if (!is_zero(throttle_scaler_inv)) {
774 2485 : throttle /= throttle_scaler_inv;
775 : }
776 : }
777 : } else {
778 : // scale steering down as speed increase above MOT_SPD_SCA_BASE (1 m/s default)
779 211060 : if (is_positive(_speed_scale_base) && (fabsf(ground_speed) > _speed_scale_base)) {
780 133213 : steering *= (_speed_scale_base / fabsf(ground_speed));
781 : } else {
782 : // regular steering rover at low speed so set limits to stop I-term build-up in controllers
783 77847 : if (!have_skid_steering()) {
784 53758 : limit.steer_left = true;
785 53758 : limit.steer_right = true;
786 : }
787 : }
788 : // reverse steering direction when backing up
789 211060 : if (is_negative(ground_speed)) {
790 23459 : steering *= -1.0f;
791 : }
792 : }
793 : } else {
794 : // reverse steering direction when backing up
795 37253 : if (is_negative(throttle)) {
796 456 : steering *= -1.0f;
797 : }
798 : }
799 250798 : output_throttle(SRV_Channel::k_throttle, throttle);
800 : } else {
801 : // handle disarmed case
802 378000 : if (_disarm_disable_pwm) {
803 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
804 : } else {
805 378000 : SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);
806 : }
807 : }
808 :
809 : // clear and set limits based on input
810 : // we do this here because vectored thrust or speed scaling may have reduced steering request
811 628798 : set_limits_from_input(armed, steering, throttle);
812 :
813 : // constrain steering
814 628798 : steering = constrain_float(steering, -4500.0f, 4500.0f);
815 :
816 : // always allow steering to move
817 628798 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
818 628798 : }
819 :
820 : // output to skid steering channels
821 628798 : void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle, float dt)
822 : {
823 628798 : if (!have_skid_steering()) {
824 581852 : return;
825 : }
826 :
827 : // clear and set limits based on input
828 87671 : set_limits_from_input(armed, steering, throttle);
829 :
830 : // constrain steering
831 87671 : steering = constrain_float(steering, -4500.0f, 4500.0f);
832 :
833 : // handle simpler disarmed case
834 87671 : if (!armed) {
835 40725 : if (_disarm_disable_pwm) {
836 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
837 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
838 : } else {
839 40725 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::TRIM);
840 40725 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::TRIM);
841 : }
842 40725 : return;
843 : }
844 :
845 : // skid steering mixer
846 46946 : float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1
847 46946 : float throttle_scaled = throttle * 0.01f; // throttle scaled -1 to +1
848 :
849 : // sanitize values for asymmetry of thrust, mixer assumes forward thrust is always larger than reverse
850 46946 : const float thrust_asymmetry = MAX(_thrust_asymmetry, 1.0);
851 46946 : const float lower_throttle_limit = -1.0 / thrust_asymmetry;
852 :
853 : // Maximum steering is half way between upper and lower limits
854 46946 : const float best_steering_throttle = (1.0 + lower_throttle_limit) * 0.5;
855 : float steering_range;
856 46946 : if (throttle_scaled < best_steering_throttle) {
857 : // steering range is reduced as throttle will never be increased by mixer
858 2238 : steering_range = MAX(throttle_scaled,0.0) - lower_throttle_limit;
859 : } else {
860 : // full range available, throttle can always be lowered down to best_steering_throttle
861 44708 : steering_range = 1 - best_steering_throttle;
862 : }
863 :
864 : // apply constraints
865 46946 : if (steering_scaled > steering_range) {
866 0 : limit.steer_right = true;
867 0 : steering_scaled = steering_range;
868 46946 : } else if (steering_scaled < -steering_range) {
869 0 : limit.steer_left = true;
870 0 : steering_scaled = -steering_range;
871 : }
872 46946 : if (throttle_scaled > 1.0) {
873 0 : limit.throttle_upper = true;
874 0 : throttle_scaled = 1.0;
875 46946 : } else if (throttle_scaled < lower_throttle_limit) {
876 0 : limit.throttle_lower = true;
877 0 : throttle_scaled = lower_throttle_limit;
878 : }
879 :
880 : // All throttle or all steering will now fit, check if they will both fit together
881 46946 : const float max_output = throttle_scaled + fabsf(steering_scaled);
882 46946 : const float min_output = throttle_scaled - fabsf(steering_scaled);
883 :
884 : // check for saturation and scale back throttle and steering proportionally
885 46946 : const float saturation_value = MAX(max_output, min_output / lower_throttle_limit);
886 46946 : if (saturation_value > 1.0f) {
887 : // store pre-scaled values so we can set limit flags afterwards
888 363 : const float steering_scaled_orig = steering_scaled;
889 363 : const float throttle_scaled_orig = throttle_scaled;
890 :
891 363 : const float str_thr_mix = constrain_float(_steering_throttle_mix, 0.0f, 1.0f);
892 363 : const float fair_scaler = 1.0f / saturation_value;
893 363 : if (str_thr_mix >= 0.5f) {
894 : // prioritise steering over throttle
895 363 : steering_scaled *= linear_interpolate(fair_scaler, 1.0f, str_thr_mix, 0.5f, 1.0f);
896 363 : if (throttle_scaled >= best_steering_throttle) {
897 : // constrained by upper limit
898 363 : throttle_scaled = 1.0 - fabsf(steering_scaled);
899 : } else {
900 : // constrained by lower limit
901 0 : throttle_scaled = fabsf(steering_scaled) + lower_throttle_limit;
902 : }
903 :
904 : } else {
905 : // prioritise throttle over steering
906 0 : throttle_scaled *= linear_interpolate(fair_scaler, 1.0f, 0.5f - str_thr_mix, 0.0f, 0.5f);
907 0 : const float steering_sign = is_positive(steering_scaled) ? 1.0 : -1.0;
908 0 : if (throttle_scaled >= best_steering_throttle) {
909 : // constrained by upper limit
910 0 : steering_scaled = (1.0 - throttle_scaled) * steering_sign;
911 : } else {
912 : // constrained by lower limit
913 0 : steering_scaled = (throttle_scaled - lower_throttle_limit) * steering_sign;
914 : }
915 : }
916 :
917 : // update limits if either steering or throttle has been reduced
918 363 : if (fabsf(steering_scaled) < fabsf(steering_scaled_orig)) {
919 363 : limit.steer_left |= is_negative(steering_scaled_orig);
920 363 : limit.steer_right |= is_positive(steering_scaled_orig);
921 : }
922 363 : if (fabsf(throttle_scaled) < fabsf(throttle_scaled_orig)) {
923 363 : limit.throttle_lower |= is_negative(throttle_scaled_orig);
924 363 : limit.throttle_upper |= is_positive(throttle_scaled_orig);
925 : }
926 : }
927 :
928 : // add in throttle and steering
929 46946 : float motor_left = throttle_scaled + steering_scaled;
930 46946 : float motor_right = throttle_scaled - steering_scaled;
931 :
932 : // Apply asymmetry correction
933 46946 : if (is_negative(motor_right)) {
934 4325 : motor_right *= thrust_asymmetry;
935 : }
936 46946 : if (is_negative(motor_left)) {
937 8694 : motor_left *= thrust_asymmetry;
938 : }
939 :
940 : // send pwm value to each motor
941 46946 : output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left, dt);
942 46946 : output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right, dt);
943 : }
944 :
945 : // output for omni frames
946 628798 : void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral)
947 : {
948 : // exit immediately if the vehicle is not omni
949 628798 : if (!is_omni()) {
950 617905 : return;
951 : }
952 :
953 10893 : if (armed) {
954 : // clear and set limits based on input
955 8746 : set_limits_from_input(armed, steering, throttle);
956 :
957 : // constrain steering
958 8746 : steering = constrain_float(steering, -4500.0f, 4500.0f);
959 :
960 : // scale throttle, steering and lateral inputs to -1 to 1
961 8746 : const float scaled_throttle = throttle * 0.01f;
962 8746 : const float scaled_steering = steering / 4500.0f;
963 8746 : const float scaled_lateral = lateral * 0.01f;
964 :
965 : // sized to the maximum rather than the runtime motor count: a
966 : // variable-length array is non-standard C++, and setup_omni() only
967 : // ever sets _motors_num to 3 or 4
968 8746 : float thr_str_ltr_out[AP_MOTORS_NUM_MOTORS_MAX] {};
969 8746 : float thr_str_ltr_max = 1;
970 34984 : for (uint8_t i=0; i<_motors_num; i++) {
971 : // Each motor outputs throttle + steering + lateral
972 26238 : thr_str_ltr_out[i] = (scaled_throttle * _throttle_factor[i]) +
973 26238 : (scaled_steering * _steering_factor[i]) +
974 26238 : (scaled_lateral * _lateral_factor[i]);
975 : // record the largest output above 1
976 26238 : if (fabsf(thr_str_ltr_out[i]) > thr_str_ltr_max) {
977 0 : thr_str_ltr_max = fabsf(thr_str_ltr_out[i]);
978 : }
979 : }
980 : // Scale all outputs back evenly such that the largest fits
981 8746 : const float output_scale = 1 / thr_str_ltr_max;
982 34984 : for (uint8_t i=0; i<_motors_num; i++) {
983 : // send output for each motor
984 26238 : output_throttle(SRV_Channels::get_motor_function(i), thr_str_ltr_out[i] * 100.0f * output_scale);
985 : }
986 8746 : if (output_scale < 1.0) {
987 : // can't tell which command resulted in the scale back, so limit all
988 0 : limit.steer_left = true;
989 0 : limit.steer_right = true;
990 0 : limit.throttle_lower = true;
991 0 : limit.throttle_upper = true;
992 : }
993 : } else {
994 : // handle disarmed case
995 2147 : if (_disarm_disable_pwm) {
996 0 : for (uint8_t i=0; i<_motors_num; i++) {
997 0 : SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::Limit::ZERO_PWM);
998 : }
999 : } else {
1000 8588 : for (uint8_t i=0; i<_motors_num; i++) {
1001 6441 : SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::Limit::TRIM);
1002 : }
1003 : }
1004 : }
1005 : }
1006 :
1007 : // return power_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
1008 : #if AP_BATTERY_WATT_MAX_ENABLED
1009 628798 : float AP_MotorsUGV::get_power_limit_max_throttle(float dt)
1010 : {
1011 628798 : AP_BattMonitor &battery = AP::battery();
1012 :
1013 : float _batt_current;
1014 628798 : const float watt_max = battery.get_watt_max();
1015 628798 : const float _batt_voltage = battery.voltage();
1016 :
1017 628798 : if (watt_max <= 0 || // return maximum if power limiting is disabled
1018 0 : !hal.util->get_soft_armed() || // remove throttle limit if disarmed
1019 628798 : !battery.current_amps(_batt_current) || // no current monitoring is available
1020 0 : is_zero(_batt_voltage)) { // no voltage monitoring is available
1021 628798 : _throttle_limit = 1.0f;
1022 628798 : return 1.0f;
1023 : }
1024 :
1025 0 : const float power = _batt_voltage * _batt_current;
1026 0 : const float power_ratio = power / watt_max;
1027 :
1028 0 : _throttle_limit += (dt / (dt + _batt_power_time_constant)) * (1.0f - power_ratio);
1029 :
1030 : // ensure throttle limit is within min and max throttle limits
1031 0 : _throttle_limit = constrain_float(_throttle_limit, _throttle_min * 0.01f, _throttle_max * 0.01f);
1032 :
1033 0 : return _throttle_limit;
1034 : }
1035 : #endif
1036 :
1037 : // output throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100
1038 370928 : void AP_MotorsUGV::output_throttle(SRV_Channel::Function function, float throttle, float dt)
1039 : {
1040 : // sanity check servo function
1041 370928 : if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3 && function!= SRV_Channel::k_motor4) {
1042 0 : return;
1043 : }
1044 :
1045 : // constrain and scale output
1046 370928 : throttle = get_scaled_throttle(throttle);
1047 :
1048 : // apply rate control
1049 370928 : throttle = get_rate_controlled_throttle(function, throttle, dt);
1050 :
1051 : // set relay if necessary
1052 : #if AP_RELAY_ENABLED
1053 370928 : AP_Relay*relay = AP::relay();
1054 370928 : if ((_pwm_type == PWMType::BRUSHED_WITH_RELAY) && (relay != nullptr)) {
1055 :
1056 : // find the output channel, if not found return
1057 0 : const SRV_Channel *out_chan = SRV_Channels::get_channel_for(function);
1058 0 : if (out_chan == nullptr) {
1059 0 : return;
1060 : }
1061 0 : const int8_t reverse_multiplier = out_chan->get_reversed() ? -1 : 1;
1062 0 : bool relay_high = is_negative(reverse_multiplier * throttle);
1063 :
1064 : AP_Relay_Params::FUNCTION relay_function;
1065 0 : switch (function) {
1066 0 : case SRV_Channel::k_throttle:
1067 : case SRV_Channel::k_throttleLeft:
1068 : case SRV_Channel::k_motor1:
1069 : default:
1070 0 : relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1;
1071 0 : break;
1072 0 : case SRV_Channel::k_throttleRight:
1073 : case SRV_Channel::k_motor2:
1074 0 : relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2;
1075 0 : break;
1076 0 : case SRV_Channel::k_motor3:
1077 0 : relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3;
1078 0 : break;
1079 0 : case SRV_Channel::k_motor4:
1080 0 : relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4;
1081 0 : break;
1082 : }
1083 0 : relay->set(relay_function, relay_high);
1084 :
1085 : // invert the output to always have positive value calculated by calc_pwm
1086 0 : throttle = reverse_multiplier * fabsf(throttle);
1087 : }
1088 : #endif // AP_RELAY_ENABLED
1089 :
1090 370928 : if (_reverse_delay > 0) {
1091 0 : switch (function) {
1092 0 : case SRV_Channel::k_throttle:
1093 0 : rev_delay_throttle.output(function, throttle, _reverse_delay);
1094 0 : return;
1095 0 : case SRV_Channel::k_throttleLeft:
1096 0 : rev_delay_throttleLeft.output(function, throttle * 10, _reverse_delay);
1097 0 : return;
1098 0 : case SRV_Channel::k_throttleRight:
1099 0 : rev_delay_throttleRight.output(function, throttle * 10, _reverse_delay);
1100 0 : return;
1101 0 : default:
1102 : // fall through to other non-delayed outputs
1103 0 : break;
1104 : }
1105 : }
1106 :
1107 : // output to servo channel
1108 370928 : switch (function) {
1109 277036 : case SRV_Channel::k_throttle:
1110 : case SRV_Channel::k_motor1:
1111 : case SRV_Channel::k_motor2:
1112 : case SRV_Channel::k_motor3:
1113 : case SRV_Channel::k_motor4:
1114 277036 : SRV_Channels::set_output_scaled(function, throttle);
1115 277036 : break;
1116 93892 : case SRV_Channel::k_throttleLeft:
1117 : case SRV_Channel::k_throttleRight:
1118 93892 : SRV_Channels::set_output_scaled(function, throttle * 10.0f);
1119 93892 : break;
1120 0 : default:
1121 : // do nothing
1122 0 : break;
1123 : }
1124 : }
1125 :
1126 : // output for sailboat's sails
1127 628798 : void AP_MotorsUGV::output_sail()
1128 : {
1129 628798 : if (!has_sail()) {
1130 604225 : return;
1131 : }
1132 :
1133 24573 : SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, _mainsail);
1134 24573 : SRV_Channels::set_output_scaled(SRV_Channel::k_wingsail_elevator, _wingsail);
1135 24573 : SRV_Channels::set_output_scaled(SRV_Channel::k_mast_rotation, _mast_rotation);
1136 : }
1137 :
1138 : // slew limit throttle for one iteration
1139 628798 : void AP_MotorsUGV::slew_limit_throttle(float dt)
1140 : {
1141 628798 : const float throttle_orig = _throttle;
1142 628798 : _throttle = get_slew_limited_throttle(_throttle, dt);
1143 628798 : if (throttle_orig > _throttle) {
1144 139 : limit.throttle_upper = true;
1145 628659 : } else if (throttle_orig < _throttle) {
1146 243 : limit.throttle_lower = true;
1147 : }
1148 628798 : _throttle_prev = _throttle;
1149 628798 : }
1150 :
1151 : // apply power limiting to throttle for one iteration
1152 : #if AP_BATTERY_WATT_MAX_ENABLED
1153 628798 : void AP_MotorsUGV::power_limit_throttle(float dt)
1154 : {
1155 628798 : const float power_limit_max = get_power_limit_max_throttle(dt);
1156 :
1157 628798 : const float throttle_max = 100.0f * power_limit_max;
1158 628798 : const float throttle_min = -100.0f * power_limit_max;
1159 :
1160 628798 : if (_throttle > throttle_max) {
1161 0 : _throttle = throttle_max;
1162 0 : limit.throttle_upper = true;
1163 628798 : } else if (_throttle < throttle_min) {
1164 0 : _throttle = throttle_min;
1165 0 : limit.throttle_lower = true;
1166 : }
1167 628798 : }
1168 : #endif
1169 :
1170 : // set limits based on steering and throttle input
1171 725215 : void AP_MotorsUGV::set_limits_from_input(bool armed, float steering, float throttle)
1172 : {
1173 : // set limits based on inputs
1174 725215 : limit.steer_left |= !armed || (steering <= -4500.0f);
1175 725215 : limit.steer_right |= !armed || (steering >= 4500.0f);
1176 725215 : limit.throttle_lower |= !armed || (throttle <= -_throttle_max);
1177 725215 : limit.throttle_upper |= !armed || (throttle >= _throttle_max);
1178 725215 : }
1179 :
1180 : // scale a throttle using the _throttle_min and _thrust_curve_expo parameters. throttle should be in the range -100 to +100
1181 370928 : float AP_MotorsUGV::get_scaled_throttle(float throttle) const
1182 : {
1183 : // exit immediately if throttle is zero
1184 370928 : if (is_zero(throttle)) {
1185 65039 : return throttle;
1186 : }
1187 :
1188 : // scale using throttle_min
1189 305889 : if (_throttle_min > 0) {
1190 0 : if (is_negative(throttle)) {
1191 0 : throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f));
1192 : } else {
1193 0 : throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f));
1194 : }
1195 : }
1196 :
1197 : // skip further scaling if thrust curve disabled or invalid
1198 305889 : if (is_zero(_thrust_curve_expo) || (_thrust_curve_expo > 1.0f) || (_thrust_curve_expo < -1.0f)) {
1199 305889 : return throttle;
1200 : }
1201 :
1202 : // calculate scaler
1203 0 : const float sign = (throttle < 0.0f) ? -1.0f : 1.0f;
1204 0 : const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) * 0.01f;
1205 0 : return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo);
1206 : }
1207 :
1208 : // use rate controller to achieve desired throttle
1209 370928 : float AP_MotorsUGV::get_rate_controlled_throttle(SRV_Channel::Function function, float throttle, float dt)
1210 : {
1211 : // require non-zero dt
1212 370928 : if (!is_positive(dt)) {
1213 277036 : return throttle;
1214 : }
1215 :
1216 : // attempt to rate control left throttle
1217 93892 : if ((function == SRV_Channel::k_throttleLeft) && _rate_controller.enabled(0)) {
1218 0 : return _rate_controller.get_rate_controlled_throttle(0, throttle, dt);
1219 : }
1220 :
1221 : // rate control right throttle
1222 93892 : if ((function == SRV_Channel::k_throttleRight) && _rate_controller.enabled(1)) {
1223 0 : return _rate_controller.get_rate_controlled_throttle(1, throttle, dt);
1224 : }
1225 :
1226 : // return throttle unchanged
1227 93892 : return throttle;
1228 : }
1229 :
1230 : // return true if motors are moving
1231 12542 : bool AP_MotorsUGV::active() const
1232 : {
1233 : // if soft disarmed, motors not active
1234 12542 : if (!hal.util->get_soft_armed()) {
1235 7518 : return false;
1236 : }
1237 :
1238 : // check throttle is active
1239 5024 : if (!is_zero(get_throttle())) {
1240 3802 : return true;
1241 : }
1242 :
1243 : // skid-steering vehicles active when steering
1244 1222 : if (have_skid_steering() && !is_zero(get_steering())) {
1245 91 : return true;
1246 : }
1247 :
1248 1131 : return false;
1249 : }
1250 :
1251 : // returns true if the configured PWM type is digital and should have fixed endpoints
1252 283 : bool AP_MotorsUGV::is_digital_pwm_type() const
1253 : {
1254 283 : switch (_pwm_type) {
1255 0 : case PWMType::DSHOT150:
1256 : case PWMType::DSHOT300:
1257 : case PWMType::DSHOT600:
1258 : case PWMType::DSHOT1200:
1259 0 : return true;
1260 283 : case PWMType::NORMAL:
1261 : case PWMType::ONESHOT:
1262 : case PWMType::ONESHOT125:
1263 : case PWMType::BRUSHED_WITH_RELAY:
1264 : case PWMType::BRUSHED_BIPOLAR:
1265 283 : break;
1266 : }
1267 283 : return false;
1268 : }
1269 :
1270 : /*
1271 : handle delay on reversal for a throttle
1272 : */
1273 0 : void AP_MotorsUGV::ReverseThrottle::output(SRV_Channel::Function function, float throttle, float delay)
1274 : {
1275 0 : const uint32_t now_ms = AP_HAL::millis();
1276 0 : if (is_zero(throttle)) {
1277 : // pass through, no change, don't update the last throttle
1278 0 : } else if (throttle * last_throttle < 0 &&
1279 0 : now_ms - last_output_ms < delay*1000) {
1280 : // sign change, add pause
1281 0 : throttle = 0;
1282 : } else {
1283 0 : last_output_ms = now_ms;
1284 0 : last_throttle = throttle;
1285 : }
1286 0 : SRV_Channels::set_output_scaled(function, throttle);
1287 0 : }
1288 :
1289 : namespace AP {
1290 566 : AP_MotorsUGV *motors_ugv()
1291 : {
1292 566 : return AP_MotorsUGV::get_singleton();
1293 : }
1294 : }
1295 :
|
