mirror of https://github.com/ArduPilot/ardupilot
39 lines
1019 B
C++
39 lines
1019 B
C++
#include "AP_Periph.h"
|
|
|
|
#ifdef HAL_PERIPH_ENABLE_RELAY
|
|
|
|
#include <dronecan_msgs.h>
|
|
|
|
void AP_Periph_FW::handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer)
|
|
{
|
|
uavcan_equipment_hardpoint_Command cmd {};
|
|
if (uavcan_equipment_hardpoint_Command_decode(transfer, &cmd)) {
|
|
// Failed to decode
|
|
return;
|
|
}
|
|
|
|
// Command must be 0 or 1, other values may be supported in the future
|
|
// rejecting them now ensures no change in behaviour
|
|
if ((cmd.command != 0) && (cmd.command != 1)) {
|
|
return;
|
|
}
|
|
|
|
// Translate hardpoint ID to relay function
|
|
AP_Relay_Params::FUNCTION fun;
|
|
switch (cmd.hardpoint_id) {
|
|
case 0 ... 15:
|
|
// 0 to 15 are continuous
|
|
fun = AP_Relay_Params::FUNCTION(cmd.hardpoint_id + (uint8_t)AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0);
|
|
break;
|
|
|
|
default:
|
|
// ID not supported
|
|
return;
|
|
}
|
|
|
|
// Set relay
|
|
relay.set(fun, cmd.command);
|
|
|
|
}
|
|
#endif
|