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.cpp - object to separate input and output channel
17 : ranges, trim and reversal
18 : */
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_Math/AP_Math.h>
22 : #include <AP_Scheduler/AP_Scheduler.h>
23 : #include <AP_Vehicle/AP_Vehicle_Type.h>
24 :
25 : #include "SRV_Channel.h"
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <AP_KDECAN/AP_KDECAN.h>
28 :
29 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS
30 : #include <AP_CANManager/AP_CANManager.h>
31 : #include <AP_DroneCAN/AP_DroneCAN.h>
32 : #include <AP_PiccoloCAN/AP_PiccoloCAN.h>
33 : #endif
34 :
35 : #if NUM_SERVO_CHANNELS == 0
36 : #pragma GCC diagnostic ignored "-Wtype-limits"
37 : #endif
38 :
39 : extern const AP_HAL::HAL& hal;
40 :
41 : SRV_Channel *SRV_Channels::channels;
42 : SRV_Channels *SRV_Channels::_singleton;
43 :
44 : uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS];
45 :
46 : uint32_t SRV_Channels::disabled_mask;
47 : uint32_t SRV_Channels::digital_mask;
48 : uint32_t SRV_Channels::reversible_mask;
49 : uint32_t SRV_Channels::invalid_mask;
50 :
51 : bool SRV_Channels::disabled_passthrough;
52 : bool SRV_Channels::initialised;
53 : bool SRV_Channels::emergency_stop;
54 : Bitmask<SRV_Channel::k_nr_aux_servo_functions> SRV_Channels::function_mask;
55 : SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];
56 : SRV_Channels::slew_list *SRV_Channels::_slew;
57 :
58 : const AP_Param::GroupInfo SRV_Channels::var_info[] = {
59 : #if (NUM_SERVO_CHANNELS >= 1)
60 : // @Group: 1_
61 : // @Path: SRV_Channel.cpp
62 : AP_SUBGROUPINFO(obj_channels[0], "1_", 1, SRV_Channels, SRV_Channel),
63 : #endif
64 :
65 : #if (NUM_SERVO_CHANNELS >= 2)
66 : // @Group: 2_
67 : // @Path: SRV_Channel.cpp
68 : AP_SUBGROUPINFO(obj_channels[1], "2_", 2, SRV_Channels, SRV_Channel),
69 : #endif
70 :
71 : #if (NUM_SERVO_CHANNELS >= 3)
72 : // @Group: 3_
73 : // @Path: SRV_Channel.cpp
74 : AP_SUBGROUPINFO(obj_channels[2], "3_", 3, SRV_Channels, SRV_Channel),
75 : #endif
76 :
77 : #if (NUM_SERVO_CHANNELS >= 4)
78 : // @Group: 4_
79 : // @Path: SRV_Channel.cpp
80 : AP_SUBGROUPINFO(obj_channels[3], "4_", 4, SRV_Channels, SRV_Channel),
81 : #endif
82 :
83 : #if (NUM_SERVO_CHANNELS >= 5)
84 : // @Group: 5_
85 : // @Path: SRV_Channel.cpp
86 : AP_SUBGROUPINFO(obj_channels[4], "5_", 5, SRV_Channels, SRV_Channel),
87 : #endif
88 :
89 : #if (NUM_SERVO_CHANNELS >= 6)
90 : // @Group: 6_
91 : // @Path: SRV_Channel.cpp
92 : AP_SUBGROUPINFO(obj_channels[5], "6_", 6, SRV_Channels, SRV_Channel),
93 : #endif
94 :
95 : #if (NUM_SERVO_CHANNELS >= 7)
96 : // @Group: 7_
97 : // @Path: SRV_Channel.cpp
98 : AP_SUBGROUPINFO(obj_channels[6], "7_", 7, SRV_Channels, SRV_Channel),
99 : #endif
100 :
101 : #if (NUM_SERVO_CHANNELS >= 8)
102 : // @Group: 8_
103 : // @Path: SRV_Channel.cpp
104 : AP_SUBGROUPINFO(obj_channels[7], "8_", 8, SRV_Channels, SRV_Channel),
105 : #endif
106 :
107 : #if (NUM_SERVO_CHANNELS >= 9)
108 : // @Group: 9_
109 : // @Path: SRV_Channel.cpp
110 : AP_SUBGROUPINFO(obj_channels[8], "9_", 9, SRV_Channels, SRV_Channel),
111 : #endif
112 :
113 : #if (NUM_SERVO_CHANNELS >= 10)
114 : // @Group: 10_
115 : // @Path: SRV_Channel.cpp
116 : AP_SUBGROUPINFO(obj_channels[9], "10_", 10, SRV_Channels, SRV_Channel),
117 : #endif
118 :
119 : #if (NUM_SERVO_CHANNELS >= 11)
120 : // @Group: 11_
121 : // @Path: SRV_Channel.cpp
122 : AP_SUBGROUPINFO(obj_channels[10], "11_", 11, SRV_Channels, SRV_Channel),
123 : #endif
124 :
125 : #if (NUM_SERVO_CHANNELS >= 12)
126 : // @Group: 12_
127 : // @Path: SRV_Channel.cpp
128 : AP_SUBGROUPINFO(obj_channels[11], "12_", 12, SRV_Channels, SRV_Channel),
129 : #endif
130 :
131 : #if (NUM_SERVO_CHANNELS >= 13)
132 : // @Group: 13_
133 : // @Path: SRV_Channel.cpp
134 : AP_SUBGROUPINFO(obj_channels[12], "13_", 13, SRV_Channels, SRV_Channel),
135 : #endif
136 :
137 : #if (NUM_SERVO_CHANNELS >= 14)
138 : // @Group: 14_
139 : // @Path: SRV_Channel.cpp
140 : AP_SUBGROUPINFO(obj_channels[13], "14_", 14, SRV_Channels, SRV_Channel),
141 : #endif
142 :
143 : #if (NUM_SERVO_CHANNELS >= 15)
144 : // @Group: 15_
145 : // @Path: SRV_Channel.cpp
146 : AP_SUBGROUPINFO(obj_channels[14], "15_", 15, SRV_Channels, SRV_Channel),
147 : #endif
148 :
149 : #if (NUM_SERVO_CHANNELS >= 16)
150 : // @Group: 16_
151 : // @Path: SRV_Channel.cpp
152 : AP_SUBGROUPINFO(obj_channels[15], "16_", 16, SRV_Channels, SRV_Channel),
153 : #endif
154 :
155 : #ifndef HAL_BUILD_AP_PERIPH
156 : // @Param{Plane}: _AUTO_TRIM
157 : // @DisplayName: Automatic servo trim
158 : // @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights. The automatic trim won't go more than 20% away from a centered trim.
159 : // @Values: 0:Disable,1:Enable
160 : // @User: Advanced
161 : AP_GROUPINFO_FRAME("_AUTO_TRIM", 17, SRV_Channels, auto_trim, 0, AP_PARAM_FRAME_PLANE),
162 : #endif
163 :
164 : // @Param: _RATE
165 : // @DisplayName: Servo default output rate
166 : // @Description: Default output rate in Hz for all PWM outputs.
167 : // @Range: 25 400
168 : // @User: Advanced
169 : // @Units: Hz
170 : AP_GROUPINFO("_RATE", 18, SRV_Channels, default_rate, 50),
171 :
172 : #if AP_VOLZ_ENABLED
173 : // @Group: _VOLZ_
174 : // @Path: ../AP_Volz_Protocol/AP_Volz_Protocol.cpp
175 : AP_SUBGROUPINFO(volz, "_VOLZ_", 19, SRV_Channels, AP_Volz_Protocol),
176 : #endif
177 :
178 : #if AP_SBUSOUTPUT_ENABLED
179 : // @Group: _SBUS_
180 : // @Path: ../AP_SBusOut/AP_SBusOut.cpp
181 : AP_SUBGROUPINFO(sbus, "_SBUS_", 20, SRV_Channels, AP_SBusOut),
182 : #endif
183 :
184 : #if HAL_SUPPORT_RCOUT_SERIAL
185 : // @Group: _BLH_
186 : // @Path: ../AP_BLHeli/AP_BLHeli.cpp
187 : AP_SUBGROUPINFO(blheli, "_BLH_", 21, SRV_Channels, AP_BLHeli),
188 : #endif
189 :
190 : #if AP_ROBOTISSERVO_ENABLED
191 : // @Group: _ROB_
192 : // @Path: ../AP_RobotisServo/AP_RobotisServo.cpp
193 : AP_SUBGROUPINFO(robotis, "_ROB_", 22, SRV_Channels, AP_RobotisServo),
194 : #endif
195 :
196 : #if AP_FETTEC_ONEWIRE_ENABLED
197 : // @Group: _FTW_
198 : // @Path: ../AP_FETtecOneWire/AP_FETtecOneWire.cpp
199 : AP_SUBGROUPINFO(fetteconwire, "_FTW_", 25, SRV_Channels, AP_FETtecOneWire),
200 : #endif
201 :
202 : // @Param: _DSHOT_RATE
203 : // @DisplayName: Servo DShot output rate
204 : // @Description: DShot output rate for all outputs as a multiple of the loop rate. 0 sets the output rate to be fixed at 1Khz for low loop rates. This value should never be set below 500Hz.
205 : // @Values: 0:1Khz,1:loop-rate,2:double loop-rate,3:triple loop-rate,4:quadruple loop rate
206 : // @User: Advanced
207 : AP_GROUPINFO("_DSHOT_RATE", 23, SRV_Channels, dshot_rate, 0),
208 :
209 : // @Param: _DSHOT_ESC
210 : // @DisplayName: Servo DShot ESC type
211 : // @Description: DShot ESC type for all outputs. The ESC type affects the range of DShot commands available and the bit widths used. None means that no dshot commands will be executed. Some ESC types support Extended DShot Telemetry (EDT) which allows telemetry other than RPM data to be returned when using bi-directional dshot. If you enable EDT you must install EDT capable firmware for correct operation.
212 : // @Values: 0:None,1:BLHeli32/Kiss/AM32,2:BLHeli_S/BlueJay,3:BLHeli32/AM32/Kiss+EDT,4:BLHeli_S/BlueJay+EDT
213 : // @User: Advanced
214 : AP_GROUPINFO("_DSHOT_ESC", 24, SRV_Channels, dshot_esc_type, 0),
215 :
216 : // @Param: _GPIO_MASK
217 : // @DisplayName: Servo GPIO mask
218 : // @Description: Bitmask of outputs which will be available as GPIOs. Any output with either the function set to -1 or with the corresponding bit set in this mask will be available for use as a GPIO pin
219 : // @Bitmask: 0:Servo 1, 1:Servo 2, 2:Servo 3, 3:Servo 4, 4:Servo 5, 5:Servo 6, 6:Servo 7, 7:Servo 8, 8:Servo 9, 9:Servo 10, 10:Servo 11, 11:Servo 12, 12:Servo 13, 13:Servo 14, 14:Servo 15, 15:Servo 16, 16:Servo 17, 17:Servo 18, 18:Servo 19, 19:Servo 20, 20:Servo 21, 21:Servo 22, 22:Servo 23, 23:Servo 24, 24:Servo 25, 25:Servo 26, 26:Servo 27, 27:Servo 28, 28:Servo 29, 29:Servo 30, 30:Servo 31, 31:Servo 32
220 : // @User: Advanced
221 : // @RebootRequired: True
222 : AP_GROUPINFO("_GPIO_MASK", 26, SRV_Channels, gpio_mask, 0),
223 :
224 : // indexes 27-43 used by SERVO_32_ENABLEd group of params
225 :
226 : // @Param: _RC_FS_MSK
227 : // @DisplayName: Servo RC Failsafe Mask
228 : // @Description: Bitmask of scaled passthru output channels which will be set to their trim value during rc failsafe instead of holding their last position before failsafe.
229 : // @Bitmask: 0:RCIN1Scaled, 1:RCIN2Scaled, 2:RCIN3Scaled, 3:RCIN4Scaled, 4:RCIN5Scaled, 5:RCIN6Scaled, 6:RCIN7Scaled, 7:RCIN8Scaled, 8:RCIN9Scaled, 9:RCIN10Scaled, 10:RCIN11Scaled, 11:SRCIN12Scaled, 12:RCIN13Scaled, 13:RCIN14Scaled, 14:RCIN15Scaled, 15:RCIN16Scaled
230 : // @User: Advanced
231 : AP_GROUPINFO("_RC_FS_MSK", 44, SRV_Channels, rc_fs_mask, 0),
232 :
233 : #if (NUM_SERVO_CHANNELS >= 17)
234 : // @Param: _32_ENABLE
235 : // @DisplayName: Enable outputs 17 to 31
236 : // @Description: This allows for up to 32 outputs, enabling parameters for outputs above 16
237 : // @User: Advanced
238 : // @Values: 0:Disabled,1:Enabled
239 : AP_GROUPINFO_FLAGS("_32_ENABLE", 43, SRV_Channels, enable_32_channels, 0, AP_PARAM_FLAG_ENABLE),
240 : #endif
241 :
242 : #if (NUM_SERVO_CHANNELS >= 17)
243 : // @Group: 17_
244 : // @Path: SRV_Channel.cpp
245 : AP_SUBGROUPINFO(obj_channels[16], "17_", 27, SRV_Channels, SRV_Channel),
246 : #endif
247 :
248 : #if (NUM_SERVO_CHANNELS >= 18)
249 : // @Group: 18_
250 : // @Path: SRV_Channel.cpp
251 : AP_SUBGROUPINFO(obj_channels[17], "18_", 28, SRV_Channels, SRV_Channel),
252 : #endif
253 :
254 : #if (NUM_SERVO_CHANNELS >= 19)
255 : // @Group: 19_
256 : // @Path: SRV_Channel.cpp
257 : AP_SUBGROUPINFO(obj_channels[18], "19_", 29, SRV_Channels, SRV_Channel),
258 : #endif
259 :
260 : #if (NUM_SERVO_CHANNELS >= 20)
261 : // @Group: 20_
262 : // @Path: SRV_Channel.cpp
263 : AP_SUBGROUPINFO(obj_channels[19], "20_", 30, SRV_Channels, SRV_Channel),
264 : #endif
265 :
266 : #if (NUM_SERVO_CHANNELS >= 21)
267 : // @Group: 21_
268 : // @Path: SRV_Channel.cpp
269 : AP_SUBGROUPINFO(obj_channels[20], "21_", 31, SRV_Channels, SRV_Channel),
270 : #endif
271 :
272 : #if (NUM_SERVO_CHANNELS >= 22)
273 : // @Group: 22_
274 : // @Path: SRV_Channel.cpp
275 : AP_SUBGROUPINFO(obj_channels[21], "22_", 32, SRV_Channels, SRV_Channel),
276 : #endif
277 :
278 : #if (NUM_SERVO_CHANNELS >= 23)
279 : // @Group: 23_
280 : // @Path: SRV_Channel.cpp
281 : AP_SUBGROUPINFO(obj_channels[22], "23_", 33, SRV_Channels, SRV_Channel),
282 : #endif
283 :
284 : #if (NUM_SERVO_CHANNELS >= 24)
285 : // @Group: 24_
286 : // @Path: SRV_Channel.cpp
287 : AP_SUBGROUPINFO(obj_channels[23], "24_", 34, SRV_Channels, SRV_Channel),
288 : #endif
289 :
290 : #if (NUM_SERVO_CHANNELS >= 25)
291 : // @Group: 25_
292 : // @Path: SRV_Channel.cpp
293 : AP_SUBGROUPINFO(obj_channels[24], "25_", 35, SRV_Channels, SRV_Channel),
294 : #endif
295 :
296 : #if (NUM_SERVO_CHANNELS >= 26)
297 : // @Group: 26_
298 : // @Path: SRV_Channel.cpp
299 : AP_SUBGROUPINFO(obj_channels[25], "26_", 36, SRV_Channels, SRV_Channel),
300 : #endif
301 :
302 : #if (NUM_SERVO_CHANNELS >= 27)
303 : // @Group: 27_
304 : // @Path: SRV_Channel.cpp
305 : AP_SUBGROUPINFO(obj_channels[26], "27_", 37, SRV_Channels, SRV_Channel),
306 : #endif
307 :
308 : #if (NUM_SERVO_CHANNELS >= 28)
309 : // @Group: 28_
310 : // @Path: SRV_Channel.cpp
311 : AP_SUBGROUPINFO(obj_channels[27], "28_", 38, SRV_Channels, SRV_Channel),
312 : #endif
313 :
314 : #if (NUM_SERVO_CHANNELS >= 29)
315 : // @Group: 29_
316 : // @Path: SRV_Channel.cpp
317 : AP_SUBGROUPINFO(obj_channels[28], "29_", 39, SRV_Channels, SRV_Channel),
318 : #endif
319 :
320 : #if (NUM_SERVO_CHANNELS >= 30)
321 : // @Group: 30_
322 : // @Path: SRV_Channel.cpp
323 : AP_SUBGROUPINFO(obj_channels[29], "30_", 40, SRV_Channels, SRV_Channel),
324 : #endif
325 :
326 : #if (NUM_SERVO_CHANNELS >= 31)
327 : // @Group: 31_
328 : // @Path: SRV_Channel.cpp
329 : AP_SUBGROUPINFO(obj_channels[30], "31_", 41, SRV_Channels, SRV_Channel),
330 : #endif
331 :
332 : #if (NUM_SERVO_CHANNELS >= 32)
333 : // @Group: 32_
334 : // @Path: SRV_Channel.cpp
335 : AP_SUBGROUPINFO(obj_channels[31], "32_", 42, SRV_Channels, SRV_Channel),
336 : #endif
337 :
338 : AP_GROUPEND
339 : };
340 :
341 : /*
342 : constructor
343 : */
344 69102 : SRV_Channels::SRV_Channels(void)
345 : {
346 2094 : _singleton = this;
347 2094 : channels = obj_channels;
348 :
349 : // set defaults from the parameter table
350 2094 : AP_Param::setup_object_defaults(this, var_info);
351 :
352 : // setup ch_num on channels
353 69102 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
354 67008 : channels[i].ch_num = i;
355 : #if NUM_SERVO_CHANNELS > 16
356 67008 : if (i >= 16) {
357 : // default to GPIO, this disables the pin and stops logging
358 33504 : channels[i].function.set_default(SRV_Channel::k_GPIO);
359 : }
360 : #endif
361 : }
362 2094 : }
363 :
364 : // SRV_Channels initialization
365 2086 : void SRV_Channels::init(uint32_t motor_mask, AP_HAL::RCOutput::output_mode mode)
366 : {
367 : // initialize BLHeli late so that all of the masks it might setup don't get trodden on by motor initialization
368 : #if HAL_SUPPORT_RCOUT_SERIAL
369 2086 : blheli.init(motor_mask, mode);
370 : #endif
371 : #ifndef HAL_BUILD_AP_PERIPH
372 2086 : hal.rcout->set_dshot_rate(_singleton->dshot_rate, AP::scheduler().get_loop_rate_hz());
373 : #endif
374 2086 : }
375 :
376 : /*
377 : save adjusted trims
378 : */
379 0 : void SRV_Channels::save_trim(void)
380 : {
381 0 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
382 0 : if (trimmed_mask & (1U<<i)) {
383 0 : channels[i].servo_trim.set_and_save(channels[i].servo_trim.get());
384 : }
385 : }
386 0 : trimmed_mask = 0;
387 0 : }
388 :
389 691 : void SRV_Channels::setup_failsafe_trim_all_non_motors(void)
390 : {
391 22803 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
392 22112 : if (!SRV_Channel::is_motor(channels[i].get_function())) {
393 21306 : hal.rcout->set_failsafe_pwm(1U<<channels[i].ch_num, channels[i].servo_trim);
394 : }
395 : }
396 691 : }
397 :
398 : /*
399 : run calc_pwm for all channels
400 : */
401 41920478 : void SRV_Channels::calc_pwm(void)
402 : {
403 : // slew rate limit functions
404 83825623 : for (slew_list *slew = _slew; slew; slew = slew->next) {
405 41905145 : if (is_positive(slew->max_change)) {
406 : // treat negative or zero slew rate as disabled
407 36796787 : functions[slew->func].output_scaled = constrain_float(functions[slew->func].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change);
408 : }
409 41905145 : slew->last_scaled_output = functions[slew->func].output_scaled;
410 : }
411 :
412 41920478 : WITH_SEMAPHORE(_singleton->override_counter_sem);
413 :
414 1383375774 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
415 : // check if channel has been locked out for this loop
416 : // if it has, decrement the loop count for that channel
417 1341455296 : if (override_counter[i] == 0) {
418 1341455296 : channels[i].set_override(false);
419 : } else {
420 0 : channels[i].set_override(true);
421 0 : override_counter[i]--;
422 : }
423 1341455296 : if (channels[i].valid_function()) {
424 671271768 : channels[i].calc_pwm(functions[channels[i].function.get()].output_scaled);
425 : }
426 : }
427 41920478 : }
428 :
429 : // set output value for a specific function channel as a pwm value
430 0 : void SRV_Channels::set_output_pwm_chan(uint8_t chan, uint16_t value)
431 : {
432 0 : if (chan < NUM_SERVO_CHANNELS) {
433 0 : channels[chan].set_output_pwm(value);
434 : }
435 0 : }
436 :
437 : // get output value for a specific channel as a pwm value
438 0 : bool SRV_Channels::get_output_pwm_chan(uint8_t chan, uint16_t &value)
439 : {
440 0 : if (chan >= NUM_SERVO_CHANNELS) {
441 0 : return false;
442 : }
443 0 : value = channels[chan].get_output_pwm();
444 0 : return true;
445 : }
446 :
447 : #if AP_SCRIPTING_ENABLED && AP_SCHEDULER_ENABLED
448 : // set output value for a specific function channel as a pwm value with loop based timeout
449 : // timeout_ms of zero will clear override of the channel
450 : // minimum override is 1 MAIN_LOOP
451 0 : void SRV_Channels::set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms)
452 : {
453 0 : WITH_SEMAPHORE(_singleton->override_counter_sem);
454 :
455 0 : if (chan < NUM_SERVO_CHANNELS) {
456 0 : const uint32_t loop_period_us = AP::scheduler().get_loop_period_us();
457 : // round up so any non-zero requested value will result in at least one loop
458 0 : const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us;
459 0 : override_counter[chan] = constrain_int32(loop_count, 0, UINT16_MAX);
460 0 : channels[chan].set_override(true);
461 0 : const bool had_pwm = SRV_Channel::have_pwm_mask & (1U<<chan);
462 0 : channels[chan].set_output_pwm(value,true);
463 0 : if (!had_pwm) {
464 : // clear the have PWM mask so the channel will default back to the scaled value when timeout expires
465 : // this is also cleared by set_output_scaled but that requires it to be re-called as some point
466 : // after the timeout is applied
467 : // note that we can't default back to a pre-override PWM value as it is not stored
468 : // checking had_pwm means the PWM will not change after the timeout, this was the existing behaviour
469 0 : SRV_Channel::have_pwm_mask &= ~(1U<<chan);
470 : }
471 : }
472 0 : }
473 : #endif // AP_SCRIPTING_ENABLED
474 :
475 : /*
476 : wrapper around hal.rcout->cork()
477 : */
478 50213822 : void SRV_Channels::cork()
479 : {
480 50213822 : hal.rcout->cork();
481 50213822 : }
482 :
483 : /*
484 : wrapper around hal.rcout->push()
485 : */
486 41692164 : void SRV_Channels::push()
487 : {
488 41692164 : hal.rcout->push();
489 :
490 : #if AP_VOLZ_ENABLED
491 : // give volz library a chance to update
492 41692164 : volz.update();
493 : #endif
494 :
495 : #if AP_SBUSOUTPUT_ENABLED
496 : // give sbus library a chance to update
497 41692164 : sbus.update();
498 : #endif
499 :
500 : #if AP_ROBOTISSERVO_ENABLED
501 : // give robotis library a chance to update
502 41692164 : robotis.update();
503 : #endif
504 :
505 : #if HAL_SUPPORT_RCOUT_SERIAL
506 : // give blheli telemetry a chance to update
507 41692164 : blheli.update_telemetry();
508 : #endif
509 :
510 : #if AP_FETTEC_ONEWIRE_ENABLED
511 41692164 : fetteconwire.update();
512 : #endif
513 :
514 : #if AP_KDECAN_ENABLED
515 41692164 : if (AP::kdecan() != nullptr) {
516 41692164 : AP::kdecan()->update();
517 : }
518 : #endif
519 :
520 : #if HAL_ENABLE_DRONECAN_DRIVERS
521 : // push outputs to CAN
522 41692164 : uint8_t can_num_drivers = AP::can().get_num_drivers();
523 125076492 : for (uint8_t i = 0; i < can_num_drivers; i++) {
524 83384328 : switch (AP::can().get_driver_type(i)) {
525 157616 : case AP_CAN::Protocol::DroneCAN: {
526 157616 : AP_DroneCAN *ap_dronecan = AP_DroneCAN::get_dronecan(i);
527 157616 : if (ap_dronecan == nullptr) {
528 0 : continue;
529 : }
530 157616 : ap_dronecan->SRV_push_servos();
531 157616 : break;
532 : }
533 : #if AP_PICCOLOCAN_ENABLED
534 0 : case AP_CAN::Protocol::PiccoloCAN: {
535 0 : AP_PiccoloCAN *ap_pcan = AP_PiccoloCAN::get_pcan(i);
536 0 : if (ap_pcan == nullptr) {
537 0 : continue;
538 : }
539 0 : ap_pcan->update();
540 0 : break;
541 : }
542 : #endif
543 83226712 : case AP_CAN::Protocol::None:
544 : default:
545 83226712 : break;
546 : }
547 : }
548 : #endif // HAL_ENABLE_DRONECAN_DRIVERS
549 41692164 : }
550 :
551 69 : void SRV_Channels::zero_rc_outputs()
552 : {
553 : /* Send an invalid signal to the motors to prevent spinning due to
554 : * neutral (1500) pwm pulse being cut short. For that matter,
555 : * send an invalid signal to all channels to prevent
556 : * undesired/unexpected behavior
557 : */
558 69 : auto &srv = AP::srv();
559 69 : srv.cork();
560 2277 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
561 2208 : hal.rcout->write(i, 0);
562 : }
563 69 : srv.push();
564 69 : }
565 :
566 : /*
567 : return true if a channel should be available as a GPIO
568 : */
569 0 : bool SRV_Channels::is_GPIO(uint8_t channel)
570 : {
571 0 : if (channel_function(channel) == SRV_Channel::k_GPIO) {
572 0 : return true;
573 : }
574 0 : if (_singleton != nullptr && (_singleton->gpio_mask & (1U<<channel)) != 0) {
575 : // user has set this channel in SERVO_GPIO_MASK
576 0 : return true;
577 : }
578 0 : return false;
579 : }
580 :
581 : // Set E - stop
582 10 : void SRV_Channels::set_emergency_stop(bool state) {
583 : #if HAL_LOGGING_ENABLED
584 10 : if (state != emergency_stop) {
585 8 : AP_Logger *logger = AP_Logger::get_singleton();
586 8 : if (logger && logger->logging_enabled()) {
587 8 : logger->Write_Event(state ? LogEvent::MOTORS_EMERGENCY_STOPPED : LogEvent::MOTORS_EMERGENCY_STOP_CLEARED);
588 : }
589 : }
590 : #endif
591 10 : emergency_stop = state;
592 10 : }
593 :
594 : namespace AP {
595 :
596 50367134 : SRV_Channels &srv()
597 : {
598 50367134 : return *SRV_Channels::get_singleton();
599 : }
600 :
601 : };
|
