diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7b2749aeec..de79b6d764 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3874,12 +3874,15 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_set_gps_global_origin(msg); break; +#if AP_MAVLINK_MSG_DEVICE_OP_ENABLED case MAVLINK_MSG_ID_DEVICE_OP_READ: handle_device_op_read(msg); break; case MAVLINK_MSG_ID_DEVICE_OP_WRITE: handle_device_op_write(msg); break; +#endif + case MAVLINK_MSG_ID_TIMESYNC: handle_timesync(msg); break; @@ -3973,9 +3976,11 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_param_value(msg); break; +#if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg); break; +#endif case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: @@ -5139,7 +5144,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_mag_cal(packet); #endif -#if AP_SERVORELAYEVENTS_ENABLED +#if AP_MAVLINK_SERVO_RELAY_ENABLED case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_SET_RELAY: diff --git a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp index b7418ad78c..ddaa450c71 100644 --- a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp +++ b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp @@ -15,6 +15,11 @@ /* handle device operations over MAVLink */ + +#include "GCS_config.h" + +#if AP_MAVLINK_MSG_DEVICE_OP_ENABLED + #include #include #include @@ -137,3 +142,5 @@ fail: packet.request_id, retcode); } + +#endif // AP_MAVLINK_MSG_DEVICE_OP_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_ServoRelay.cpp b/libraries/GCS_MAVLink/GCS_ServoRelay.cpp index 105d27f50f..c624268763 100644 --- a/libraries/GCS_MAVLink/GCS_ServoRelay.cpp +++ b/libraries/GCS_MAVLink/GCS_ServoRelay.cpp @@ -2,7 +2,7 @@ #include "AP_ServoRelayEvents/AP_ServoRelayEvents.h" -#if AP_SERVORELAYEVENTS_ENABLED +#if AP_MAVLINK_SERVO_RELAY_ENABLED MAV_RESULT GCS_MAVLINK::handle_servorelay_message(const mavlink_command_int_t &packet) { @@ -48,4 +48,4 @@ MAV_RESULT GCS_MAVLINK::handle_servorelay_message(const mavlink_command_int_t &p return result; } -#endif // AP_SERVORELAYEVENTS_ENABLED +#endif // AP_MAVLINK_SERVO_RELAY_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 28e8ddd22a..e3bb5ed7b0 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -61,3 +61,16 @@ #ifndef AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED #define AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED HAL_GCS_ENABLED #endif + +// this is for both read and write messages: +#ifndef AP_MAVLINK_MSG_DEVICE_OP_ENABLED +#define AP_MAVLINK_MSG_DEVICE_OP_ENABLED HAL_GCS_ENABLED +#endif + +#ifndef AP_MAVLINK_SERVO_RELAY_ENABLED +#define AP_MAVLINK_SERVO_RELAY_ENABLED HAL_GCS_ENABLED && AP_SERVORELAYEVENTS_ENABLED +#endif + +#ifndef AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED +#define AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED HAL_GCS_ENABLED +#endif diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index 133297dbcb..50322e2e5d 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/libraries/GCS_MAVLink/GCS_serial_control.cpp @@ -17,6 +17,9 @@ along with this program. If not, see . */ +#include "GCS_config.h" + +#if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED #include #include "GCS.h" @@ -199,3 +202,5 @@ more_data: goto more_data; } } + +#endif // AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED