Line data Source code
1 : /*
2 : * AP_Relay.cpp
3 : *
4 : * Created on: Oct 2, 2011
5 : * Author: Amilcar Lucas
6 : */
7 :
8 : #include "AP_Relay_config.h"
9 :
10 : #if AP_RELAY_ENABLED
11 :
12 : #include "AP_Relay.h"
13 :
14 : #include <AP_HAL/AP_HAL.h>
15 : #include <AP_Math/AP_Math.h>
16 : #include <GCS_MAVLink/GCS.h>
17 : #include <AP_Logger/AP_Logger.h>
18 :
19 : #include <AP_ICEngine/AP_ICEngine.h>
20 : #include <AP_Parachute/AP_Parachute.h>
21 : #include <AP_Camera/AP_Camera.h>
22 :
23 : #include <AP_Vehicle/AP_Vehicle_Type.h>
24 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
25 : #include <AR_Motors/AP_MotorsUGV.h>
26 : #endif
27 :
28 : #if AP_RELAY_DRONECAN_ENABLED
29 : #include <AP_DroneCAN/AP_DroneCAN.h>
30 : #include <AP_CANManager/AP_CANManager.h>
31 : #endif
32 :
33 : #if AP_SIM_ENABLED
34 : #include <SITL/SITL.h>
35 : #endif
36 :
37 : #ifndef RELAY1_PIN_DEFAULT
38 : #define RELAY1_PIN_DEFAULT -1
39 : #endif
40 :
41 : #ifndef RELAY2_PIN_DEFAULT
42 : #define RELAY2_PIN_DEFAULT -1
43 : #endif
44 :
45 : #ifndef RELAY3_PIN_DEFAULT
46 : #define RELAY3_PIN_DEFAULT -1
47 : #endif
48 :
49 : #ifndef RELAY4_PIN_DEFAULT
50 : #define RELAY4_PIN_DEFAULT -1
51 : #endif
52 :
53 : #ifndef RELAY5_PIN_DEFAULT
54 : #define RELAY5_PIN_DEFAULT -1
55 : #endif
56 :
57 : #ifndef RELAY6_PIN_DEFAULT
58 : #define RELAY6_PIN_DEFAULT -1
59 : #endif
60 :
61 : #ifndef RELAY1_DEFAULT
62 : #define RELAY1_DEFAULT -1
63 : #endif
64 :
65 : #ifndef RELAY2_DEFAULT
66 : #define RELAY2_DEFAULT -1
67 : #endif
68 :
69 : #ifndef RELAY3_DEFAULT
70 : #define RELAY3_DEFAULT -1
71 : #endif
72 :
73 : #ifndef RELAY4_DEFAULT
74 : #define RELAY4_DEFAULT -1
75 : #endif
76 :
77 : #ifndef RELAY5_DEFAULT
78 : #define RELAY5_DEFAULT -1
79 : #endif
80 :
81 : #ifndef RELAY6_DEFAULT
82 : #define RELAY6_DEFAULT -1
83 : #endif
84 :
85 : static constexpr bool relay_default_valid(const int8_t state)
86 : {
87 : // -1 means no hwdef-level default was specified; the parameter keeps its compiled-in default
88 : return (state == -1) ||
89 : ((state >= int8_t(AP_Relay_Params::DefaultState::OFF)) &&
90 : (state <= int8_t(AP_Relay_Params::DefaultState::NO_CHANGE)));
91 : }
92 :
93 : static_assert(relay_default_valid(RELAY1_DEFAULT), "Invalid RELAY1_DEFAULT");
94 : static_assert(relay_default_valid(RELAY2_DEFAULT), "Invalid RELAY2_DEFAULT");
95 : static_assert(relay_default_valid(RELAY3_DEFAULT), "Invalid RELAY3_DEFAULT");
96 : static_assert(relay_default_valid(RELAY4_DEFAULT), "Invalid RELAY4_DEFAULT");
97 : static_assert(relay_default_valid(RELAY5_DEFAULT), "Invalid RELAY5_DEFAULT");
98 : static_assert(relay_default_valid(RELAY6_DEFAULT), "Invalid RELAY6_DEFAULT");
99 :
100 :
101 : const AP_Param::GroupInfo AP_Relay::var_info[] = {
102 : // 0 was PIN
103 : // 1 was PIN2
104 : // 2 was PIN3
105 : // 3 was PIN4
106 : // 4 was DEFAULT
107 : // 5 was PIN5
108 : // 6 was PIN6
109 :
110 : // @Group: 1_
111 : // @Path: AP_Relay_Params.cpp
112 : AP_SUBGROUPINFO(_params[0], "1_", 7, AP_Relay, AP_Relay_Params),
113 :
114 : #if AP_RELAY_NUM_RELAYS > 1
115 : // @Group: 2_
116 : // @Path: AP_Relay_Params.cpp
117 : AP_SUBGROUPINFO(_params[1], "2_", 8, AP_Relay, AP_Relay_Params),
118 : #endif
119 :
120 : #if AP_RELAY_NUM_RELAYS > 2
121 : // @Group: 3_
122 : // @Path: AP_Relay_Params.cpp
123 : AP_SUBGROUPINFO(_params[2], "3_", 9, AP_Relay, AP_Relay_Params),
124 : #endif
125 :
126 : #if AP_RELAY_NUM_RELAYS > 3
127 : // @Group: 4_
128 : // @Path: AP_Relay_Params.cpp
129 : AP_SUBGROUPINFO(_params[3], "4_", 10, AP_Relay, AP_Relay_Params),
130 : #endif
131 :
132 : #if AP_RELAY_NUM_RELAYS > 4
133 : // @Group: 5_
134 : // @Path: AP_Relay_Params.cpp
135 : AP_SUBGROUPINFO(_params[4], "5_", 11, AP_Relay, AP_Relay_Params),
136 : #endif
137 :
138 : #if AP_RELAY_NUM_RELAYS > 5
139 : // @Group: 6_
140 : // @Path: AP_Relay_Params.cpp
141 : AP_SUBGROUPINFO(_params[5], "6_", 12, AP_Relay, AP_Relay_Params),
142 : #endif
143 :
144 : #if AP_RELAY_NUM_RELAYS > 6
145 : // @Group: 7_
146 : // @Path: AP_Relay_Params.cpp
147 : AP_SUBGROUPINFO(_params[6], "7_", 13, AP_Relay, AP_Relay_Params),
148 : #endif
149 :
150 : #if AP_RELAY_NUM_RELAYS > 7
151 : // @Group: 8_
152 : // @Path: AP_Relay_Params.cpp
153 : AP_SUBGROUPINFO(_params[7], "8_", 14, AP_Relay, AP_Relay_Params),
154 : #endif
155 :
156 : #if AP_RELAY_NUM_RELAYS > 8
157 : // @Group: 9_
158 : // @Path: AP_Relay_Params.cpp
159 : AP_SUBGROUPINFO(_params[8], "9_", 15, AP_Relay, AP_Relay_Params),
160 : #endif
161 :
162 : #if AP_RELAY_NUM_RELAYS > 9
163 : // @Group: 10_
164 : // @Path: AP_Relay_Params.cpp
165 : AP_SUBGROUPINFO(_params[9], "10_", 16, AP_Relay, AP_Relay_Params),
166 : #endif
167 :
168 : #if AP_RELAY_NUM_RELAYS > 10
169 : // @Group: 11_
170 : // @Path: AP_Relay_Params.cpp
171 : AP_SUBGROUPINFO(_params[10], "11_", 17, AP_Relay, AP_Relay_Params),
172 : #endif
173 :
174 : #if AP_RELAY_NUM_RELAYS > 11
175 : // @Group: 12_
176 : // @Path: AP_Relay_Params.cpp
177 : AP_SUBGROUPINFO(_params[11], "12_", 18, AP_Relay, AP_Relay_Params),
178 : #endif
179 :
180 : #if AP_RELAY_NUM_RELAYS > 12
181 : // @Group: 13_
182 : // @Path: AP_Relay_Params.cpp
183 : AP_SUBGROUPINFO(_params[12], "13_", 19, AP_Relay, AP_Relay_Params),
184 : #endif
185 :
186 : #if AP_RELAY_NUM_RELAYS > 13
187 : // @Group: 14_
188 : // @Path: AP_Relay_Params.cpp
189 : AP_SUBGROUPINFO(_params[13], "14_", 20, AP_Relay, AP_Relay_Params),
190 : #endif
191 :
192 : #if AP_RELAY_NUM_RELAYS > 14
193 : // @Group: 15_
194 : // @Path: AP_Relay_Params.cpp
195 : AP_SUBGROUPINFO(_params[14], "15_", 21, AP_Relay, AP_Relay_Params),
196 : #endif
197 :
198 : #if AP_RELAY_NUM_RELAYS > 15
199 : // @Group: 16_
200 : // @Path: AP_Relay_Params.cpp
201 : AP_SUBGROUPINFO(_params[15], "16_", 22, AP_Relay, AP_Relay_Params),
202 : #endif
203 :
204 : AP_GROUPEND
205 : };
206 :
207 : AP_Relay *AP_Relay::singleton;
208 :
209 : extern const AP_HAL::HAL& hal;
210 :
211 14686 : AP_Relay::AP_Relay(void)
212 : {
213 2098 : AP_Param::setup_object_defaults(this, var_info);
214 :
215 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
216 2096 : if (singleton != nullptr) {
217 0 : AP_HAL::panic("AP_Relay must be singleton");
218 : }
219 : #endif
220 2098 : singleton = this;
221 2098 : }
222 :
223 2059 : void AP_Relay::convert_params()
224 : {
225 : // PARAMETER_CONVERSION - Added: Dec-2023
226 : #ifndef HAL_BUILD_AP_PERIPH
227 : // Dont need this conversion on periph as relay support is more recent
228 :
229 : // Before converting local params we must find any relays being used by index from external libs
230 2059 : int8_t ice_relay = -1;
231 : #if AP_ICENGINE_ENABLED
232 2059 : AP_ICEngine *ice = AP::ice();
233 : int8_t ice_relay_index;
234 2059 : if (ice != nullptr && ice->get_legacy_ignition_relay_index(ice_relay_index)) {
235 0 : ice_relay = ice_relay_index;
236 : }
237 : #endif
238 :
239 2059 : int8_t chute_relay = -1;
240 : #if HAL_PARACHUTE_ENABLED
241 2059 : AP_Parachute *parachute = AP::parachute();
242 : int8_t parachute_relay_index;
243 2059 : if (parachute != nullptr && parachute->get_legacy_relay_index(parachute_relay_index)) {
244 39 : chute_relay = parachute_relay_index;
245 : }
246 : #endif
247 :
248 2059 : int8_t cam_relay = -1;
249 : #if AP_CAMERA_ENABLED
250 2059 : AP_Camera *camera = AP::camera();
251 : int8_t camera_relay_index;
252 2059 : if ((camera != nullptr) && (camera->get_legacy_relay_index(camera_relay_index))) {
253 0 : cam_relay = camera_relay_index;
254 : }
255 : #endif
256 :
257 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
258 283 : int8_t rover_relay[] = { -1, -1, -1, -1 };
259 283 : AP_MotorsUGV *motors = AP::motors_ugv();
260 283 : if (motors != nullptr) {
261 283 : motors->get_legacy_relay_index(rover_relay[0], rover_relay[1], rover_relay[2], rover_relay[3]);
262 : }
263 : #endif
264 :
265 : // Find old default param
266 2059 : AP_Relay_Params::DefaultState default_state = AP_Relay_Params::DefaultState::OFF; // off was the old behaviour
267 2059 : const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state);
268 :
269 : // grab the old values if they were set
270 14413 : for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), 6U); i++) {
271 12354 : if (_params[i].function.configured()) {
272 : // Conversion already done, or user has configured manually
273 12031 : continue;
274 : }
275 :
276 10281 : uint8_t param_index = i;
277 10281 : if (i > 3) {
278 : // Skip over the old DEFAULT parameter at index 4
279 4064 : param_index += 1;
280 : }
281 :
282 10281 : int8_t pin = 0;
283 10281 : if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) {
284 : // Copy old pin parameter if valid
285 0 : _params[i].pin.set_and_save(pin);
286 : }
287 :
288 : // Work out what function this relay should be
289 : AP_Relay_Params::FUNCTION new_fun;
290 10281 : if (i == chute_relay) {
291 0 : new_fun = AP_Relay_Params::FUNCTION::PARACHUTE;
292 :
293 10281 : } else if (i == ice_relay) {
294 0 : new_fun = AP_Relay_Params::FUNCTION::IGNITION;
295 :
296 10281 : } else if (i == cam_relay) {
297 0 : new_fun = AP_Relay_Params::FUNCTION::CAMERA;
298 :
299 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
300 1142 : } else if (i == rover_relay[0]) {
301 0 : new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1;
302 :
303 1142 : } else if (i == rover_relay[1]) {
304 0 : new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2;
305 :
306 1142 : } else if (i == rover_relay[2]) {
307 0 : new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3;
308 :
309 1142 : } else if (i == rover_relay[3]) {
310 0 : new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4;
311 : #endif
312 :
313 : } else {
314 10281 : if (_params[i].pin < 0) {
315 : // Don't enable as numbered relay if pin is invalid
316 : // Other functions should be enabled with a invalid pin
317 : // This will result in a pre-arm promoting the user to resolve
318 9958 : continue;
319 : }
320 323 : new_fun = AP_Relay_Params::FUNCTION::RELAY;
321 :
322 : }
323 323 : _params[i].function.set_and_save(new_fun);
324 :
325 :
326 : // Set the default state
327 323 : if (have_default) {
328 0 : _params[i].default_state.set_and_save(default_state);
329 : }
330 : }
331 : #endif // HAL_BUILD_AP_PERIPH
332 2059 : }
333 :
334 2059 : void AP_Relay::set_defaults() {
335 2059 : const int8_t pins[] = { RELAY1_PIN_DEFAULT,
336 : RELAY2_PIN_DEFAULT,
337 : RELAY3_PIN_DEFAULT,
338 : RELAY4_PIN_DEFAULT,
339 : RELAY5_PIN_DEFAULT,
340 : RELAY6_PIN_DEFAULT };
341 2059 : const int8_t default_states[] = { RELAY1_DEFAULT,
342 : RELAY2_DEFAULT,
343 : RELAY3_DEFAULT,
344 : RELAY4_DEFAULT,
345 : RELAY5_DEFAULT,
346 : RELAY6_DEFAULT };
347 :
348 : static_assert(ARRAY_SIZE(pins) == ARRAY_SIZE(default_states));
349 :
350 14413 : for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), ARRAY_SIZE(pins)); i++) {
351 : // set the default
352 12354 : if (pins[i] != -1) {
353 2059 : _params[i].pin.set_default(pins[i]);
354 : }
355 12354 : if (default_states[i] != -1) {
356 0 : _params[i].default_state.set_default(default_states[i]);
357 : }
358 : }
359 2059 : }
360 :
361 : // Return true is function is valid
362 811099 : bool AP_Relay::function_valid(AP_Relay_Params::FUNCTION function) const
363 : {
364 811099 : return (function > AP_Relay_Params::FUNCTION::NONE) && (function < AP_Relay_Params::FUNCTION::NUM_FUNCTIONS);
365 : }
366 :
367 2059 : void AP_Relay::init()
368 : {
369 2059 : set_defaults();
370 :
371 2059 : convert_params();
372 :
373 : // setup the actual default values of all the pins
374 14413 : for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
375 12354 : const int16_t pin = _params[instance].pin;
376 12354 : if (pin == -1) {
377 : // no valid pin to set it on, skip it
378 10009 : continue;
379 : }
380 :
381 2345 : const AP_Relay_Params::FUNCTION function = _params[instance].function;
382 2345 : if (!function_valid(function)) {
383 : // invalid function, skip it
384 0 : continue;
385 : }
386 :
387 2345 : bool use_default_param = (function == AP_Relay_Params::FUNCTION::RELAY);
388 : #ifdef HAL_BUILD_AP_PERIPH
389 : use_default_param |= (function >= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0 && function <= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_15);
390 : #endif
391 2345 : if (use_default_param) {
392 : // relay by instance number, set the state to match our output
393 2345 : const AP_Relay_Params::DefaultState default_state = _params[instance].default_state;
394 2345 : if ((default_state == AP_Relay_Params::DefaultState::OFF) ||
395 : (default_state == AP_Relay_Params::DefaultState::ON)) {
396 :
397 2345 : set_instance_state(instance, (bool)default_state);
398 : }
399 : } else {
400 : // all functions are supposed to be off by default
401 0 : set_instance_state(instance, false);
402 : }
403 :
404 : // Make sure any DroneCAN pin is enabled for streaming
405 : #if AP_RELAY_DRONECAN_ENABLED
406 2345 : dronecan.enable_pin(pin);
407 : #endif
408 :
409 : }
410 2059 : }
411 :
412 23021 : void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) {
413 23021 : if (!function_valid(function)) {
414 : // invalid function
415 0 : return;
416 : }
417 :
418 161147 : for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
419 138126 : if (function != _params[instance].function) {
420 138126 : continue;
421 : }
422 :
423 0 : set_instance_state(instance, value);
424 : }
425 : }
426 :
427 : // set a pins output state by instance and log if required
428 : // this is an internal helper, instance must have already been validated to be in range
429 2434 : void AP_Relay::set_instance_state(uint8_t instance, bool value)
430 : {
431 2434 : const int16_t pin = _params[instance].pin;
432 2434 : if (pin == -1) {
433 : // no valid pin to set it on, skip it
434 0 : return;
435 : }
436 :
437 : #if AP_SIM_ENABLED
438 2434 : if (!(AP::sitl()->on_hardware_relay_enable_mask & (1U << instance))) {
439 0 : return;
440 : }
441 : #endif
442 :
443 2434 : const bool initial_value = get_pin_state(pin);
444 :
445 2434 : if (_params[instance].inverted > 0) {
446 0 : value = !value;
447 : }
448 :
449 2434 : if (initial_value != value) {
450 604 : set_pin_state(pin, value);
451 : #if HAL_LOGGING_ENABLED
452 : // @LoggerMessage: RELY
453 : // @Description: Relay state
454 : // @Field: TimeUS: Time since system startup
455 : // @Field: Instance: relay instance number
456 : // @Field: State: current state
457 604 : AP::logger().Write(
458 : "RELY",
459 : "TimeUS," "Instance," "State",
460 : "s" "#" "-",
461 : "F" "-" "-",
462 : "Q" "B" "B",
463 : AP_HAL::micros64(),
464 : instance,
465 : value);
466 : #endif // HAL_LOGGING_ENABLED
467 : }
468 : }
469 :
470 89 : void AP_Relay::set(const uint8_t instance, const bool value)
471 : {
472 89 : if (instance >= ARRAY_SIZE(_params)) {
473 0 : return;
474 : }
475 :
476 89 : if (_params[instance].function != AP_Relay_Params::FUNCTION::RELAY) {
477 0 : return;
478 : }
479 :
480 89 : set_instance_state(instance, value);
481 : }
482 :
483 40 : void AP_Relay::toggle(uint8_t instance)
484 : {
485 40 : if (instance < ARRAY_SIZE(_params)) {
486 40 : set(instance, !get(instance));
487 : }
488 40 : }
489 :
490 : // check settings are valid
491 66519 : bool AP_Relay::arming_checks(size_t buflen, char *buffer) const
492 : {
493 465633 : for (uint8_t i=0; i<ARRAY_SIZE(_params); i++) {
494 399114 : if (!function_valid(_params[i].function)) {
495 : // Relay disabled
496 319005 : continue;
497 : }
498 :
499 80109 : const int16_t pin = _params[i].pin.get();
500 80109 : if (pin == -1) {
501 : // Pin disabled, may want to pre-arm this in the future as function enabled with invalid pin
502 : // User should set function to none to disable
503 158 : continue;
504 : }
505 :
506 : #if AP_RELAY_DRONECAN_ENABLED
507 79951 : const bool DroneCAN_pin = dronecan.valid_pin(pin);
508 : #else
509 : const bool DroneCAN_pin = false;
510 : #endif
511 :
512 79951 : if (!DroneCAN_pin && !hal.gpio->valid_pin(pin)) {
513 : // Check GPIO pin is valid
514 : char param_name_buf[14];
515 0 : hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY%u_PIN", unsigned(i+1));
516 : uint8_t servo_ch;
517 0 : if (hal.gpio->pin_to_servo_channel(pin, servo_ch)) {
518 0 : hal.util->snprintf(buffer, buflen, "%s=%d, set SERVO%u_FUNCTION=-1", param_name_buf, int(pin), unsigned(servo_ch+1));
519 : } else {
520 0 : hal.util->snprintf(buffer, buflen, "%s=%d invalid", param_name_buf, int(pin));
521 : }
522 0 : return false;
523 : }
524 :
525 : // Each pin can only be used by a single relay
526 464308 : for (uint8_t j=i+1; j<ARRAY_SIZE(_params); j++) {
527 384357 : if (!function_valid((AP_Relay_Params::FUNCTION)_params[j].function.get())) {
528 : // Relay disabled
529 368999 : continue;
530 : }
531 15358 : if (pin == _params[j].pin.get()) {
532 0 : hal.util->snprintf(buffer, buflen, "pin conflict RELAY%u_PIN = RELAY%u_PIN", int(i+1), int(j+1));
533 0 : return false;
534 : }
535 : }
536 : }
537 66519 : return true;
538 : }
539 :
540 1090 : bool AP_Relay::get(uint8_t instance) const
541 : {
542 1090 : if (instance >= ARRAY_SIZE(_params)) {
543 : // invalid instance
544 0 : return false;
545 : }
546 :
547 1090 : if (_params[instance].inverted > 0) {
548 0 : return !get_pin_state(_params[instance].pin.get());
549 : }
550 :
551 1090 : return get_pin_state(_params[instance].pin.get());
552 : }
553 :
554 : // Get relay state from pin number
555 3524 : bool AP_Relay::get_pin_state(const int16_t pin) const
556 : {
557 3524 : if (pin < 0) {
558 : // invalid pin
559 2 : return false;
560 : }
561 :
562 : #if AP_RELAY_DRONECAN_ENABLED
563 3522 : if (dronecan.valid_pin(pin)) {
564 : // Virtual DroneCAN pin
565 0 : return dronecan.get_pin_state(pin);
566 : }
567 : #endif
568 :
569 : // Real GPIO pin
570 3522 : hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
571 3522 : return (bool)hal.gpio->read(pin);
572 : }
573 :
574 : // Set relay state from pin number
575 604 : void AP_Relay::set_pin_state(const int16_t pin, const bool value)
576 : {
577 604 : if (pin < 0) {
578 : // invalid pin
579 0 : return;
580 : }
581 :
582 : #if AP_RELAY_DRONECAN_ENABLED
583 604 : if (dronecan.valid_pin(pin)) {
584 : // Virtual DroneCAN pin
585 0 : dronecan.set_pin_state(pin, value);
586 0 : return;
587 : }
588 : #endif
589 :
590 : // Real GPIO pin
591 604 : hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
592 604 : hal.gpio->write(pin, value);
593 : }
594 :
595 : // Get GPIO pin from instance
596 492 : bool AP_Relay::get_pin_by_instance(uint8_t instance, uint8_t &pin) const
597 : {
598 492 : if (instance >= ARRAY_SIZE(_params)) {
599 0 : return false;
600 : }
601 492 : pin = _params[instance].pin;
602 492 : return true;
603 : }
604 : // see if the relay is enabled
605 53 : bool AP_Relay::enabled(uint8_t instance) const
606 : {
607 : // Must be a valid instance with function relay and pin set
608 53 : return (instance < ARRAY_SIZE(_params)) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY);
609 : }
610 :
611 : // see if the relay is enabled
612 0 : bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const
613 : {
614 0 : for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
615 0 : if ((_params[instance].function == function) && (_params[instance].pin != -1)) {
616 0 : return true;
617 : }
618 : }
619 0 : return false;
620 : }
621 :
622 : #if AP_RELAY_DRONECAN_ENABLED
623 : // Return true if pin number is a virtual DroneCAN pin
624 86422 : bool AP_Relay::DroneCAN::valid_pin(int16_t pin) const
625 : {
626 86422 : switch(pin) {
627 0 : case (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0 ... (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15:
628 0 : return true;
629 86422 : default:
630 86422 : return false;
631 : }
632 : }
633 :
634 : // Enable streaming of pin number
635 2345 : void AP_Relay::DroneCAN::enable_pin(int16_t pin)
636 : {
637 2345 : if (!valid_pin(pin)) {
638 2345 : return;
639 : }
640 :
641 0 : const uint8_t index = hardpoint_index(pin);
642 0 : state[index].enabled = true;
643 : }
644 :
645 : // Get the hardpoint index of given pin number
646 0 : uint8_t AP_Relay::DroneCAN::hardpoint_index(const int16_t pin) const
647 : {
648 0 : return pin - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0;
649 : }
650 :
651 : // Set DroneCAN relay state from pin number
652 0 : void AP_Relay::DroneCAN::set_pin_state(const int16_t pin, const bool value)
653 : {
654 0 : const uint8_t index = hardpoint_index(pin);
655 :
656 : // Set pin and ensure enabled for streaming
657 0 : state[index].enabled = true;
658 0 : state[index].value = value;
659 :
660 : // Broadcast msg on all channels
661 : // Just a single send, rely on streaming to fill in any lost packet
662 :
663 0 : uavcan_equipment_hardpoint_Command msg {};
664 0 : msg.hardpoint_id = index;
665 0 : msg.command = state[index].value;
666 :
667 0 : uint8_t can_num_drivers = AP::can().get_num_drivers();
668 0 : for (uint8_t i = 0; i < can_num_drivers; i++) {
669 0 : auto *ap_dronecan = AP_DroneCAN::get_dronecan(i);
670 0 : if (ap_dronecan != nullptr) {
671 0 : ap_dronecan->relay_hardpoint.broadcast(msg);
672 : }
673 : }
674 :
675 0 : }
676 :
677 : // Get relay state from pin number, this relies on a cached value, assume remote pin is in sync
678 0 : bool AP_Relay::DroneCAN::get_pin_state(const int16_t pin) const
679 : {
680 0 : const uint8_t index = hardpoint_index(pin);
681 0 : return state[index].value;
682 : }
683 :
684 : // Populate message and update index with the sent command
685 : // This will allow the caller to cycle through each enabled pin
686 0 : bool AP_Relay::DroneCAN::populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const
687 : {
688 : // Find the next enabled index
689 0 : for (uint8_t i = 0; i < ARRAY_SIZE(state); i++) {
690 : // Look for next index, wrapping back to 0 as needed
691 0 : const uint8_t new_index = (index + 1 + i) % ARRAY_SIZE(state);
692 0 : if (!state[new_index].enabled) {
693 : // This index is not being used
694 0 : continue;
695 : }
696 :
697 : // Update command and index then return
698 0 : msg.hardpoint_id = new_index;
699 0 : msg.command = state[new_index].value;
700 0 : index = new_index;
701 0 : return true;
702 : }
703 :
704 0 : return false;
705 : }
706 : #endif // AP_RELAY_DRONECAN_ENABLED
707 :
708 : #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
709 : // this method may only return false if there is no space in the
710 : // supplied link for the message.
711 377 : bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const
712 : {
713 : static_assert(AP_RELAY_NUM_RELAYS <= 16, "Too many relays for MAVLink status reporting to work.");
714 :
715 377 : if (!HAVE_PAYLOAD_SPACE(link.get_chan(), RELAY_STATUS)) {
716 0 : return false;
717 : }
718 :
719 377 : uint16_t present_mask = 0;
720 377 : uint16_t on_mask = 0;
721 2639 : for (uint8_t i=0; i<ARRAY_SIZE(_params); i++) {
722 2262 : if (!function_valid(_params[i].function)) {
723 1212 : continue;
724 : }
725 1050 : const uint16_t relay_bit_mask = 1U << i;
726 1050 : present_mask |= relay_bit_mask;
727 :
728 1050 : if (get(i)) {
729 279 : on_mask |= relay_bit_mask;
730 : }
731 : }
732 :
733 377 : mavlink_msg_relay_status_send(
734 : link.get_chan(),
735 : AP_HAL::millis(),
736 : on_mask,
737 : present_mask
738 : );
739 377 : return true;
740 : }
741 : #endif // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
742 :
743 : namespace AP {
744 :
745 476164 : AP_Relay *relay()
746 : {
747 476164 : return AP_Relay::get_singleton();
748 : }
749 :
750 : }
751 :
752 : #endif // AP_RELAY_ENABLED
|
