ardupilot/libraries/GCS_MAVLink/GCS_ServoRelay.cpp

46 lines
1.2 KiB
C++

#include "GCS.h"
#include "AP_ServoRelayEvents/AP_ServoRelayEvents.h"
MAV_RESULT GCS_MAVLINK::handle_servorelay_message(mavlink_command_long_t &packet)
{
AP_ServoRelayEvents *handler = AP::servorelayevents();
if (handler == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
MAV_RESULT result = MAV_RESULT_FAILED;
switch (packet.command) {
case MAV_CMD_DO_SET_SERVO:
if (handler->do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_SERVO:
if (handler->do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4 * 1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_RELAY:
if (handler->do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
if (handler->do_repeat_relay(packet.param1, packet.param2, packet.param3 * 1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
default:
result = MAV_RESULT_UNSUPPORTED;
break;
}
return result;
}