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 : SRV_Channel_aux.cpp - handling of servo auxiliary functions
17 : */
18 : #include "SRV_Channel.h"
19 :
20 : #include <AP_Math/AP_Math.h>
21 : #include <AP_HAL/AP_HAL.h>
22 : #include <RC_Channel/RC_Channel.h>
23 : #include <GCS_MAVLink/GCS.h>
24 :
25 : #if NUM_SERVO_CHANNELS == 0
26 : #pragma GCC diagnostic ignored "-Wtype-limits"
27 : #endif
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : /// map a function to a servo channel and output it
32 835609922 : void SRV_Channel::output_ch(void)
33 : {
34 : #ifndef HAL_BUILD_AP_PERIPH
35 835609922 : int8_t passthrough_from = -1;
36 835609922 : bool passthrough_mapped = false;
37 :
38 : // take care of special function cases
39 835609922 : switch(function.get())
40 : {
41 0 : case k_manual: // manual
42 0 : passthrough_from = ch_num;
43 0 : break;
44 428 : case k_rcin1 ... k_rcin16: // rc pass-thru
45 428 : passthrough_from = int8_t((int16_t)function - k_rcin1);
46 428 : break;
47 10772 : case k_rcin1_mapped ... k_rcin16_mapped:
48 10772 : passthrough_from = int8_t((int16_t)function - k_rcin1_mapped);
49 10772 : passthrough_mapped = true;
50 10772 : break;
51 : }
52 835609922 : if (passthrough_from != -1) {
53 : // we are doing passthrough from input to output for this channel
54 11200 : RC_Channel *c = rc().channel(passthrough_from);
55 11200 : if (c) {
56 11200 : if (SRV_Channels::passthrough_disabled()) {
57 0 : output_pwm = c->get_radio_trim();
58 : } else {
59 : // non-mapped rc passthrough
60 11200 : int16_t radio_in = c->get_radio_in();
61 11200 : if (passthrough_mapped) {
62 10772 : if (rc().has_valid_input()) {
63 5965 : switch (c->get_type()) {
64 5965 : case RC_Channel::ControlType::ANGLE:
65 5965 : radio_in = pwm_from_angle(c->norm_input_dz() * 4500);
66 5965 : break;
67 0 : case RC_Channel::ControlType::RANGE:
68 : // convert RC normalised input from -1 to +1 range to 0 to +1 and output as range
69 0 : radio_in = pwm_from_range((c->norm_input_ignore_trim() + 1.0) * 0.5 * 4500);
70 0 : break;
71 : }
72 : } else {
73 : // no valid input. If we are in radio
74 : // failsafe then go to trim values (if
75 : // configured for this channel). Otherwise
76 : // use the last-good value
77 4807 : if ( ((1U<<passthrough_from) & SRV_Channels::get_rc_fs_mask()) && rc().in_rc_failsafe()) {
78 4791 : radio_in = pwm_from_angle(0);
79 : } else {
80 16 : radio_in = previous_radio_in;
81 : }
82 : }
83 : }
84 11200 : if (!ign_small_rcin_changes) {
85 11022 : output_pwm = radio_in;
86 11022 : previous_radio_in = radio_in;
87 : } else {
88 : // check if rc input value has changed by more than the deadzone
89 178 : if (abs(radio_in - previous_radio_in) > c->get_dead_zone()) {
90 0 : output_pwm = radio_in;
91 0 : ign_small_rcin_changes = false;
92 : }
93 : }
94 : }
95 : }
96 : }
97 : #endif // HAL_BUILD_AP_PERIPH
98 :
99 835609922 : if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
100 835609922 : hal.rcout->write(ch_num, output_pwm);
101 : }
102 835609922 : }
103 :
104 : /*
105 : call output_ch() on all channels
106 : */
107 41920478 : void SRV_Channels::output_ch_all(void)
108 : {
109 41920478 : uint8_t max_chan = NUM_SERVO_CHANNELS;
110 : #if NUM_SERVO_CHANNELS >= 17
111 41920478 : if (_singleton != nullptr && _singleton->enable_32_channels.get() <= 0) {
112 41919712 : max_chan = 16;
113 : }
114 : #endif
115 712660382 : for (uint8_t i = 0; i < max_chan; i++) {
116 670739904 : channels[i].output_ch();
117 : }
118 41920478 : }
119 :
120 : /*
121 : return the current function for a channel
122 : */
123 5043739 : SRV_Channel::Function SRV_Channels::channel_function(uint8_t channel)
124 : {
125 5043739 : if (channel < NUM_SERVO_CHANNELS) {
126 5043739 : return channels[channel].function;
127 : }
128 0 : return SRV_Channel::k_none;
129 : }
130 :
131 : /*
132 : setup a channels aux servo function
133 : */
134 2476451 : void SRV_Channel::aux_servo_function_setup(void)
135 : {
136 2476451 : if (type_setup) {
137 308587 : return;
138 : }
139 2167864 : switch (function.get()) {
140 4 : case k_flap:
141 : case k_flap_auto:
142 : case k_egg_drop:
143 : case k_lift_release:
144 4 : set_range(100);
145 4 : break;
146 92 : case k_heli_rsc:
147 : case k_heli_tail_rsc:
148 : case k_motor_tilt:
149 : case k_boost_throttle:
150 : case k_thrust_out:
151 92 : set_range(1000);
152 92 : break;
153 2112 : case k_aileron_with_input:
154 : case k_elevator_with_input:
155 : case k_aileron:
156 : case k_elevator:
157 : case k_dspoilerLeft1:
158 : case k_dspoilerLeft2:
159 : case k_dspoilerRight1:
160 : case k_dspoilerRight2:
161 : case k_rudder:
162 : case k_steering:
163 : case k_flaperon_left:
164 : case k_flaperon_right:
165 : case k_tiltMotorLeft:
166 : case k_tiltMotorRight:
167 : case k_tiltMotorRear:
168 : case k_tiltMotorRearLeft:
169 : case k_tiltMotorRearRight:
170 : case k_elevon_left:
171 : case k_elevon_right:
172 : case k_vtail_left:
173 : case k_vtail_right:
174 : case k_scripting1:
175 : case k_scripting2:
176 : case k_scripting3:
177 : case k_scripting4:
178 : case k_scripting5:
179 : case k_scripting6:
180 : case k_scripting7:
181 : case k_scripting8:
182 : case k_scripting9:
183 : case k_scripting10:
184 : case k_scripting11:
185 : case k_scripting12:
186 : case k_scripting13:
187 : case k_scripting14:
188 : case k_scripting15:
189 : case k_scripting16:
190 : case k_roll_out:
191 : case k_pitch_out:
192 : case k_yaw_out:
193 : case k_rcin1_mapped ... k_rcin16_mapped:
194 2112 : set_angle(4500);
195 2112 : break;
196 683 : case k_throttle:
197 : case k_throttleLeft:
198 : case k_throttleRight:
199 : case k_airbrake:
200 : // fixed wing throttle
201 683 : set_range(100);
202 683 : break;
203 83 : case k_cam_zoom:
204 : case k_cam_focus:
205 : case k_lights1:
206 : case k_lights2:
207 : case k_video_switch:
208 83 : set_range(1000);
209 83 : break;
210 : #if ACTUATOR_CHANNELS > 0
211 0 : case k_actuator1 ... k_actuator6:
212 : // We take floats from -1 to 1. see MAV_CMD_DO_SET_ACTUATOR
213 0 : set_angle(1);
214 0 : break;
215 : #endif
216 2164890 : default:
217 2164890 : break;
218 : }
219 : }
220 :
221 : /// setup the output range types of all functions
222 154360 : void SRV_Channels::update_aux_servo_function(void)
223 : {
224 154360 : if (!channels) {
225 0 : return;
226 : }
227 154360 : function_mask.clearall();
228 :
229 29482760 : for (uint16_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
230 29328400 : functions[i].channel_mask = 0;
231 : }
232 154360 : invalid_mask = 0;
233 :
234 : // set auxiliary ranges
235 5093880 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
236 4939520 : if (!channels[i].valid_function()) {
237 2468547 : invalid_mask |= 1U<<i;
238 2468547 : continue;
239 : }
240 2470973 : const uint16_t function = channels[i].function.get();
241 2470973 : channels[i].aux_servo_function_setup();
242 2470973 : function_mask.set(function);
243 2470973 : functions[function].channel_mask |= 1U<<i;
244 : }
245 154360 : initialised = true;
246 : }
247 :
248 : /// Should be called after the servo functions have been initialized
249 : /// called at 1Hz
250 151226 : void SRV_Channels::enable_aux_servos()
251 : {
252 151226 : hal.rcout->set_default_rate(uint16_t(_singleton->default_rate.get()));
253 :
254 151226 : update_aux_servo_function();
255 :
256 : // enable all channels that are set to a valid function. This
257 : // includes k_none servos, which allows those to get their initial
258 : // trim value on startup
259 4990458 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
260 4839232 : SRV_Channel &c = channels[i];
261 : // see if it is a valid function
262 4839232 : if (c.valid_function() && !(disabled_mask & (1U<<c.ch_num))) {
263 2420960 : hal.rcout->enable_ch(c.ch_num);
264 : } else {
265 2418272 : hal.rcout->disable_ch(c.ch_num);
266 : }
267 :
268 : // output some servo functions before we fiddle with the
269 : // parameter values:
270 4839232 : if (c.function == SRV_Channel::k_min) {
271 0 : c.set_output_pwm(c.servo_min);
272 0 : c.output_ch();
273 4839232 : } else if (c.function == SRV_Channel::k_trim) {
274 0 : c.set_output_pwm(c.servo_trim);
275 0 : c.output_ch();
276 4839232 : } else if (c.function == SRV_Channel::k_max) {
277 0 : c.set_output_pwm(c.servo_max);
278 0 : c.output_ch();
279 : }
280 : }
281 :
282 : // propagate channel masks to the ESCS
283 151226 : hal.rcout->update_channel_masks();
284 :
285 : #if HAL_SUPPORT_RCOUT_SERIAL
286 151226 : blheli.update();
287 : #endif
288 151226 : }
289 :
290 : /*
291 : for channels which have been marked as digital output then the
292 : MIN/MAX/TRIM values have no meaning for controlling output, as
293 : the HAL handles the scaling. We still need to cope with places
294 : in the code that may try to set a PWM value however, so to
295 : ensure consistency we force the MIN/MAX/TRIM to be consistent
296 : across all digital channels. We use a MIN/MAX of 1000/2000, and
297 : set TRIM to either 1000 or 1500 depending on whether the channel
298 : is reversible
299 : */
300 2103 : void SRV_Channels::set_digital_outputs(uint32_t dig_mask, uint32_t rev_mask) {
301 2103 : digital_mask |= dig_mask;
302 2103 : reversible_mask |= rev_mask;
303 :
304 : // add in NeoPixel and ProfiLED functions to digital array to determine anything else
305 : // that should be disabled
306 69399 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
307 67296 : SRV_Channel &c = channels[i];
308 67296 : switch (c.function.get()) {
309 0 : case SRV_Channel::k_LED_neopixel1:
310 : case SRV_Channel::k_LED_neopixel2:
311 : case SRV_Channel::k_LED_neopixel3:
312 : case SRV_Channel::k_LED_neopixel4:
313 : case SRV_Channel::k_ProfiLED_1:
314 : case SRV_Channel::k_ProfiLED_2:
315 : case SRV_Channel::k_ProfiLED_3:
316 : case SRV_Channel::k_ProfiLED_Clock:
317 0 : dig_mask |= 1U<<c.ch_num;
318 0 : break;
319 67296 : default:
320 67296 : break;
321 : }
322 : }
323 2103 : disabled_mask = hal.rcout->get_disabled_channels(dig_mask);
324 :
325 69399 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
326 67296 : SRV_Channel &c = channels[i];
327 67296 : if (digital_mask & (1U<<i)) {
328 61 : c.servo_min.set_and_default(1000);
329 61 : c.servo_max.set_and_default(2000);
330 61 : if (reversible_mask & (1U<<i)) {
331 0 : c.servo_trim.set_and_default(1500);
332 : } else {
333 61 : c.servo_trim.set_and_default(1000);
334 : }
335 : }
336 : }
337 2103 : }
338 :
339 : /// enable output channels using a channel mask
340 0 : void SRV_Channels::enable_by_mask(uint32_t mask)
341 : {
342 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
343 0 : if (mask & (1U<<i)) {
344 0 : hal.rcout->enable_ch(i);
345 : }
346 : }
347 0 : }
348 :
349 : /*
350 : set radio_out for all channels matching the given function type
351 : */
352 155916375 : void SRV_Channels::set_output_pwm(SRV_Channel::Function function, uint16_t value)
353 : {
354 155916375 : if (!function_assigned(function)) {
355 203465 : return;
356 : }
357 : // channels is a pointer assigned from obj_channels:
358 5138526030 : for (uint8_t i = 0; i < ARRAY_SIZE(obj_channels); i++) {
359 4982813120 : if (channels[i].function == function) {
360 155712827 : channels[i].set_output_pwm(value);
361 155712827 : channels[i].output_ch();
362 : }
363 : }
364 : }
365 :
366 : /*
367 : set radio_out for all channels matching the given function type
368 : trim the output assuming a 1500 center on the given value
369 : reverses pwm output based on channel reversed property
370 : */
371 : void
372 9157191 : SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Function function, int16_t value)
373 : {
374 9157191 : if (!function_assigned(function)) {
375 0 : return;
376 : }
377 302187303 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
378 293030112 : if (channels[i].function == function) {
379 : int16_t value2;
380 9157191 : if (channels[i].get_reversed()) {
381 0 : value2 = 1500 - value + channels[i].get_trim();
382 : } else {
383 9157191 : value2 = value - 1500 + channels[i].get_trim();
384 : }
385 9157191 : channels[i].set_output_pwm(constrain_int16(value2,channels[i].get_output_min(),channels[i].get_output_max()));
386 9157191 : channels[i].output_ch();
387 : }
388 : }
389 : }
390 :
391 : /*
392 : set and save the trim value to current output for all channels matching
393 : the given function type
394 : */
395 : void
396 0 : SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Function function)
397 : {
398 0 : if (!function_assigned(function)) {
399 0 : return;
400 : }
401 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
402 0 : if (channels[i].function == function) {
403 0 : channels[i].servo_trim.set_and_save_ifchanged(channels[i].get_output_pwm());
404 : }
405 : }
406 : }
407 :
408 : #if AP_RC_CHANNEL_ENABLED
409 : /*
410 : copy radio_in to radio_out for a given function
411 : */
412 : void
413 60433 : SRV_Channels::copy_radio_in_out(SRV_Channel::Function function, bool do_input_output)
414 : {
415 60433 : if (!function_assigned(function)) {
416 60433 : return;
417 : }
418 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
419 0 : if (channels[i].function == function) {
420 0 : RC_Channel *c = rc().channel(channels[i].ch_num);
421 0 : if (c == nullptr) {
422 0 : continue;
423 : }
424 0 : channels[i].set_output_pwm(c->get_radio_in());
425 0 : if (do_input_output) {
426 0 : channels[i].output_ch();
427 : }
428 : }
429 : }
430 : }
431 :
432 : /*
433 : copy radio_in to radio_out for a channel mask
434 : */
435 : void
436 0 : SRV_Channels::copy_radio_in_out_mask(uint32_t mask)
437 : {
438 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
439 0 : if ((1U<<i) & mask) {
440 0 : RC_Channel *c = rc().channel(channels[i].ch_num);
441 0 : if (c == nullptr) {
442 0 : continue;
443 : }
444 0 : channels[i].set_output_pwm(c->get_radio_in());
445 : }
446 : }
447 :
448 0 : }
449 : #endif // AP_RC_CHANNEL_ENABLED
450 :
451 : /*
452 : setup failsafe value for an auxiliary function type to a Limit
453 : */
454 : void
455 0 : SRV_Channels::set_failsafe_pwm(SRV_Channel::Function function, uint16_t pwm)
456 : {
457 0 : if (!function_assigned(function)) {
458 0 : return;
459 : }
460 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
461 0 : const SRV_Channel &c = channels[i];
462 0 : if (c.function == function) {
463 0 : hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
464 : }
465 : }
466 : }
467 :
468 : /*
469 : setup failsafe value for an auxiliary function type to a Limit
470 : */
471 : void
472 28468 : SRV_Channels::set_failsafe_limit(SRV_Channel::Function function, SRV_Channel::Limit limit)
473 : {
474 28468 : if (!function_assigned(function)) {
475 16817 : return;
476 : }
477 384483 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
478 372832 : const SRV_Channel &c = channels[i];
479 372832 : if (c.function == function) {
480 11744 : uint16_t pwm = c.get_limit_pwm(limit);
481 11744 : hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
482 : }
483 : }
484 : }
485 :
486 : /*
487 : set radio output value for an auxiliary function type to a Limit
488 : */
489 : void
490 3701877 : SRV_Channels::set_output_limit(SRV_Channel::Function function, SRV_Channel::Limit limit)
491 : {
492 3701877 : if (!function_assigned(function)) {
493 3280047 : return;
494 : }
495 13920390 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
496 13498560 : SRV_Channel &c = channels[i];
497 13498560 : if (c.function == function) {
498 425163 : uint16_t pwm = c.get_limit_pwm(limit);
499 425163 : c.set_output_pwm(pwm);
500 : #if AP_RC_CHANNEL_ENABLED
501 425163 : if (c.function == SRV_Channel::k_manual) {
502 0 : RC_Channel *cin = rc().channel(c.ch_num);
503 0 : if (cin != nullptr) {
504 : // in order for output_ch() to work for k_manual we
505 : // also have to override radio_in
506 0 : cin->set_radio_in(pwm);
507 : }
508 : }
509 : #endif
510 : }
511 : }
512 : }
513 :
514 : /*
515 : return true if a particular function is assigned to at least one RC channel
516 : */
517 : bool
518 177686292 : SRV_Channels::function_assigned(SRV_Channel::Function function)
519 : {
520 177686292 : if (!initialised) {
521 1475 : update_aux_servo_function();
522 : }
523 177686292 : return function_mask.get(uint16_t(function));
524 : }
525 :
526 : /*
527 : set servo_out and angle_min/max, then calc_pwm and output a
528 : value. This is used to move a AP_Mount servo
529 : */
530 : void
531 1427537 : SRV_Channels::move_servo(SRV_Channel::Function function,
532 : int16_t value, int16_t angle_min, int16_t angle_max)
533 : {
534 1427537 : if (!function_assigned(function)) {
535 957165 : return;
536 : }
537 470372 : if (angle_max <= angle_min) {
538 0 : return;
539 : }
540 470372 : float v = float(value - angle_min) / float(angle_max - angle_min);
541 470372 : v = constrain_float(v, 0.0f, 1.0f);
542 15522276 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
543 15051904 : SRV_Channel &c = channels[i];
544 15051904 : if (c.function == function) {
545 738676 : float v2 = c.get_reversed()? (1-v) : v;
546 738676 : uint16_t pwm = c.servo_min + v2 * (c.servo_max - c.servo_min);
547 738676 : c.set_output_pwm(pwm);
548 : }
549 : }
550 : }
551 :
552 : /*
553 : set the default channel an auxiliary output function should be on
554 : */
555 2585672 : bool SRV_Channels::set_aux_channel_default(SRV_Channel::Function function, uint8_t channel)
556 : {
557 2585672 : if (function_assigned(function)) {
558 : // already assigned
559 2580190 : return true;
560 : }
561 5498 : if (channels[channel].function != SRV_Channel::k_none &&
562 16 : !(channel >15 && channels[channel].function == SRV_Channel::k_GPIO)) {
563 4 : if (channels[channel].function == function) {
564 0 : return true;
565 : }
566 8 : hal.console->printf("Channel %u already assigned function %u\n",
567 4 : (unsigned)(channel + 1),
568 4 : (unsigned)channels[channel].function.get());
569 4 : return false;
570 : }
571 5478 : channels[channel].type_setup = false;
572 5478 : channels[channel].function.set_and_default(function);
573 5478 : channels[channel].aux_servo_function_setup();
574 5478 : function_mask.set((uint16_t)function);
575 5478 : if (SRV_Channel::valid_function(function)) {
576 5478 : functions[function].channel_mask |= 1U<<channel;
577 : }
578 5478 : return true;
579 : }
580 :
581 : // find first channel that a function is assigned to
582 13814328 : bool SRV_Channels::find_channel(SRV_Channel::Function function, uint8_t &chan)
583 : {
584 : // Must have populated channel masks
585 13814328 : if (!initialised) {
586 493 : update_aux_servo_function();
587 : }
588 :
589 : // Make sure function is valid
590 13814328 : if (!SRV_Channel::valid_function(function)) {
591 0 : return false;
592 : }
593 :
594 : // Get the index of the first set bit, returns 0 if no bits are set
595 13814328 : const int first_chan = __builtin_ffs(functions[function].channel_mask);
596 13814328 : if (first_chan == 0) {
597 13259080 : return false;
598 : }
599 :
600 : // Convert to 0 indexed
601 555248 : chan = first_chan - 1;
602 555248 : return true;
603 : }
604 :
605 : /*
606 : get a pointer to first auxiliary channel for a channel function
607 : */
608 873 : SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Function function)
609 : {
610 : uint8_t chan;
611 873 : if (!find_channel(function, chan)) {
612 2 : return nullptr;
613 : }
614 871 : return &channels[chan];
615 : }
616 :
617 384117364 : void SRV_Channels::set_output_scaled(SRV_Channel::Function function, float value)
618 : {
619 384117364 : if (SRV_Channel::valid_function(function)) {
620 384117364 : functions[function].output_scaled = value;
621 384117364 : SRV_Channel::have_pwm_mask &= ~functions[function].channel_mask;
622 : }
623 384117364 : }
624 :
625 168865593 : float SRV_Channels::get_output_scaled(SRV_Channel::Function function)
626 : {
627 168865593 : if (SRV_Channel::valid_function(function)) {
628 168865593 : return functions[function].output_scaled;
629 : }
630 0 : return 0;
631 : }
632 :
633 : // get slew limited scaled output for the given function type
634 10461453 : float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Function function)
635 : {
636 10461453 : if (!SRV_Channel::valid_function(function)) {
637 0 : return 0.0;
638 : }
639 52077316 : for (slew_list *slew = _slew; slew; slew = slew->next) {
640 52020347 : if (slew->func == function) {
641 10404484 : if (!is_positive(slew->max_change)) {
642 : // treat negative or zero slew rate as disabled
643 151 : break;
644 : }
645 10404333 : return constrain_float(functions[function].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change);
646 : }
647 : }
648 : // no slew limiting
649 57120 : return functions[function].output_scaled;
650 : }
651 :
652 : /*
653 : get mask of output channels for a function
654 : */
655 3577633 : uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Function function)
656 : {
657 3577633 : if (!initialised) {
658 82 : update_aux_servo_function();
659 : }
660 3577633 : if (SRV_Channel::valid_function(function)) {
661 186570 : return functions[function].channel_mask;
662 : }
663 3391063 : return invalid_mask;
664 : }
665 :
666 :
667 : // set the trim for a function channel to given pwm
668 0 : void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Function function, int16_t pwm)
669 : {
670 0 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
671 0 : if (channels[i].function == function) {
672 0 : channels[i].servo_trim.set_and_default(pwm);
673 : }
674 : }
675 0 : }
676 :
677 : // set the trim for a function channel to min output of the channel honnoring reverse unless ignore_reversed is true
678 2076 : void SRV_Channels::set_trim_to_min_for(SRV_Channel::Function function, bool ignore_reversed)
679 : {
680 68508 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
681 66432 : if (channels[i].function == function) {
682 683 : channels[i].servo_trim.set_and_default((channels[i].get_reversed() && !ignore_reversed)?channels[i].servo_max:channels[i].servo_min);
683 : }
684 : }
685 2076 : }
686 :
687 : /*
688 : set the default function for a channel
689 : */
690 3438 : void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Function function)
691 : {
692 3438 : if (chan < NUM_SERVO_CHANNELS) {
693 3438 : const SRV_Channel::Function old = channels[chan].function;
694 3438 : channels[chan].function.set_default(function);
695 3438 : if (old != channels[chan].function && channels[chan].function == function) {
696 3333 : function_mask.set((uint16_t)function);
697 : }
698 : }
699 3438 : }
700 :
701 112642 : void SRV_Channels::set_esc_scaling_for(SRV_Channel::Function function)
702 : {
703 : uint8_t chan;
704 112642 : if (find_channel(function, chan)) {
705 42835 : hal.rcout->set_esc_scaling(channels[chan].get_output_min(), channels[chan].get_output_max());
706 : }
707 112642 : }
708 :
709 : /*
710 : auto-adjust channel trim from an integrator value. Positive v means
711 : adjust trim up. Negative means decrease
712 : */
713 0 : void SRV_Channels::adjust_trim(SRV_Channel::Function function, float v)
714 : {
715 0 : if (is_zero(v)) {
716 0 : return;
717 : }
718 0 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
719 0 : SRV_Channel &c = channels[i];
720 0 : if (function != c.function) {
721 0 : continue;
722 : }
723 0 : float change = c.reversed?-v:v;
724 0 : uint16_t new_trim = c.servo_trim;
725 0 : if (c.servo_max <= c.servo_min) {
726 0 : continue;
727 : }
728 0 : float trim_scaled = float(c.servo_trim - c.servo_min) / (c.servo_max - c.servo_min);
729 0 : if (change > 0 && trim_scaled < 0.6f) {
730 0 : new_trim++;
731 0 : } else if (change < 0 && trim_scaled > 0.4f) {
732 0 : new_trim--;
733 : } else {
734 0 : continue;
735 : }
736 0 : c.servo_trim.set(new_trim);
737 :
738 0 : trimmed_mask |= 1U<<i;
739 : }
740 : }
741 :
742 : // get pwm output for the first channel of the given function type.
743 61 : bool SRV_Channels::get_output_pwm(SRV_Channel::Function function, uint16_t &value)
744 : {
745 : uint8_t chan;
746 61 : if (!find_channel(function, chan)) {
747 0 : return false;
748 : }
749 61 : if (!SRV_Channel::valid_function(function)) {
750 0 : return false;
751 : }
752 61 : channels[chan].calc_pwm(functions[function].output_scaled);
753 61 : value = channels[chan].get_output_pwm();
754 61 : return true;
755 : }
756 :
757 : // set output pwm to trim for the given function
758 597660 : void SRV_Channels::set_output_to_trim(SRV_Channel::Function function)
759 : {
760 19722780 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
761 19125120 : if (channels[i].function == function) {
762 3363 : channels[i].set_output_pwm(channels[i].servo_trim);
763 : }
764 : }
765 597660 : }
766 :
767 : /*
768 : get the normalised output for a channel function from the pwm value
769 : of the first matching channel
770 : */
771 208276 : float SRV_Channels::get_output_norm(SRV_Channel::Function function)
772 : {
773 : uint8_t chan;
774 208276 : if (!find_channel(function, chan)) {
775 104138 : return 0;
776 : }
777 104138 : if (SRV_Channel::valid_function(function)) {
778 104138 : channels[chan].calc_pwm(functions[function].output_scaled);
779 : }
780 104138 : return channels[chan].get_output_norm();
781 : }
782 :
783 : // set normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max) for the given function
784 0 : void SRV_Channels::set_output_norm(SRV_Channel::Function function, float value)
785 : {
786 0 : if (!function_assigned(function)) {
787 0 : return;
788 : }
789 0 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
790 0 : SRV_Channel &c = channels[i];
791 0 : if (c.function == function) {
792 0 : c.set_output_norm(value);
793 : }
794 : }
795 : }
796 :
797 : /*
798 : limit slew rate for an output function to given rate in percent per
799 : second. This assumes output has not yet done to the hal
800 : */
801 41886826 : void SRV_Channels::set_slew_rate(SRV_Channel::Function function, float slew_rate, uint16_t range, float dt)
802 : {
803 41886826 : if (!SRV_Channel::valid_function(function)) {
804 0 : return;
805 : }
806 41886826 : const float max_change = range * slew_rate * 0.01 * dt;
807 :
808 125661387 : for (slew_list *slew = _slew; slew; slew = slew->next) {
809 125657932 : if (slew->func == function) {
810 : // found existing item, update max change
811 41883371 : slew->max_change = max_change;
812 41883371 : return;
813 : }
814 : }
815 :
816 : // The slew limit is still allocated even if the slew rate is 0 for disabled.
817 : // This means that if the slew limit is changed at some point in the future we still have
818 : // a valid last output to slew from.
819 :
820 : // add new item
821 3455 : slew_list *new_slew = NEW_NOTHROW slew_list(function);
822 3455 : if (new_slew == nullptr) {
823 0 : return;
824 : }
825 3455 : new_slew->last_scaled_output = functions[function].output_scaled;
826 3455 : new_slew->max_change = max_change;
827 3455 : new_slew->next = _slew;
828 3455 : _slew = new_slew;
829 : }
830 :
831 : // update channels last_scaled_output to match value
832 7 : void SRV_Channels::set_slew_last_scaled_output(SRV_Channel::Function function, float value)
833 : {
834 21 : for (slew_list *slew = _slew; slew; slew = slew->next) {
835 21 : if (slew->func == function) {
836 : // found existing item, update slew limiter value
837 7 : slew->last_scaled_output = value;
838 7 : return;
839 : }
840 : }
841 : }
842 :
843 : // call set_angle() on matching channels
844 497687 : void SRV_Channels::set_angle(SRV_Channel::Function function, uint16_t angle)
845 : {
846 16423671 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
847 15925984 : if (channels[i].function == function) {
848 433173 : channels[i].set_angle(angle);
849 : }
850 : }
851 497687 : }
852 :
853 : // call set_range() on matching channels
854 2589038 : void SRV_Channels::set_range(SRV_Channel::Function function, uint16_t range)
855 : {
856 85438254 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
857 82849216 : if (channels[i].function == function) {
858 2579714 : channels[i].set_range(range);
859 : }
860 : }
861 2589038 : }
862 :
863 : // set MIN parameter for a function
864 2579539 : void SRV_Channels::set_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm)
865 : {
866 85124787 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
867 82545248 : if (channels[i].function == function) {
868 2579539 : channels[i].set_output_min(min_pwm);
869 2579539 : channels[i].set_output_max(max_pwm);
870 : }
871 : }
872 2579539 : }
873 :
874 : // set MIN/MAX parameter defaults for a function
875 1382 : void SRV_Channels::set_output_min_max_defaults(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm)
876 : {
877 45606 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
878 44224 : if (channels[i].function == function) {
879 16 : channels[i].servo_min.set_default(min_pwm);
880 16 : channels[i].servo_max.set_default(max_pwm);
881 : }
882 : }
883 1382 : }
884 :
885 : // Save MIN/MAX/REVERSED parameters for a function
886 12 : void SRV_Channels::save_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm)
887 : {
888 396 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
889 384 : if (channels[i].function == function) {
890 : // If min is larger than max swap and set reversed
891 12 : const bool reversed = min_pwm > max_pwm;
892 12 : channels[i].servo_min.set_and_save(reversed ? max_pwm : min_pwm);
893 12 : channels[i].servo_max.set_and_save(reversed ? min_pwm : max_pwm);
894 12 : channels[i].reversed.set_and_save(reversed ? 1 : 0);
895 : }
896 : }
897 12 : }
898 :
899 : // constrain to output min/max for function
900 3410 : void SRV_Channels::constrain_pwm(SRV_Channel::Function function)
901 : {
902 112530 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
903 109120 : SRV_Channel &c = channels[i];
904 109120 : if (c.function == function) {
905 3410 : c.set_output_pwm(constrain_int16(c.output_pwm, c.servo_min, c.servo_max));
906 : }
907 : }
908 3410 : }
909 :
910 : // set RC output frequency on a function output
911 4 : void SRV_Channels::set_rc_frequency(SRV_Channel::Function function, uint16_t frequency_hz)
912 : {
913 4 : uint32_t mask = 0;
914 132 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
915 128 : SRV_Channel &c = channels[i];
916 128 : if (c.function == function) {
917 0 : mask |= (1U<<c.ch_num);
918 : }
919 : }
920 4 : if (mask != 0) {
921 0 : hal.rcout->set_freq(mask, frequency_hz);
922 : }
923 4 : }
|
