From e423173848f640142c3d59f6c101be00372aed64 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 6 Jun 2023 18:05:06 +1000 Subject: [PATCH] AP_Mission: add option to disable relay and servorelay libraries --- libraries/AP_Mission/AP_Mission.cpp | 3 +++ libraries/AP_Mission/AP_Mission_Commands.cpp | 7 +++++++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 143f485fd0..67de677799 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -9,6 +9,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_Mission::var_info[] = { @@ -404,11 +405,13 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_DO_GRIPPER: return start_command_do_gripper(cmd); #endif +#if AP_SERVORELAYEVENTS_ENABLED case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: return start_command_do_servorelayevents(cmd); +#endif #if AP_CAMERA_ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index cf156932b6..aa2da85dc9 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -59,6 +59,7 @@ bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd } #endif // AP_GRIPPER_ENABLED +#if AP_SERVORELAYEVENTS_ENABLED bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd) { AP_ServoRelayEvents *sre = AP::servorelayevents(); @@ -70,8 +71,10 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com case MAV_CMD_DO_SET_SERVO: return sre->do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm); +#if AP_RELAY_ENABLED case MAV_CMD_DO_SET_RELAY: return sre->do_set_relay(cmd.content.relay.num, cmd.content.relay.state); +#endif case MAV_CMD_DO_REPEAT_SERVO: return sre->do_repeat_servo(cmd.content.repeat_servo.channel, @@ -79,10 +82,13 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f); +#if AP_RELAY_ENABLED case MAV_CMD_DO_REPEAT_RELAY: return sre->do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count, cmd.content.repeat_relay.cycle_time * 1000.0f); +#endif + default: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("Unhandled servo/relay case"); @@ -90,6 +96,7 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com return false; } } +#endif // AP_SERVORELAYEVENTS_ENABLED #if AP_CAMERA_ENABLED bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)