From 78fcf7057c6c23eb312a14b5d6f6599f87208827 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 May 2024 17:32:06 +1000 Subject: [PATCH] GCS_MAVLink: move switch for sending camera messages into AP_Camera neatens GCS_Common a bit, reduces repetitive code --- libraries/GCS_MAVLink/GCS_Common.cpp | 42 ++-------------------------- 1 file changed, 2 insertions(+), 40 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ad782c46ca..2c9dc7cc30 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6014,46 +6014,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if AP_CAMERA_ENABLED case MSG_CAMERA_FEEDBACK: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); - camera->send_feedback(chan); - } - break; case MSG_CAMERA_INFORMATION: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_INFORMATION); - camera->send_camera_information(chan); - } - break; case MSG_CAMERA_SETTINGS: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_SETTINGS); - camera->send_camera_settings(chan); - } - break; #if AP_CAMERA_SEND_FOV_STATUS_ENABLED case MSG_CAMERA_FOV_STATUS: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_FOV_STATUS); - camera->send_camera_fov_status(chan); - } - break; #endif case MSG_CAMERA_CAPTURE_STATUS: { @@ -6061,11 +6025,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) if (camera == nullptr) { break; } - CHECK_PAYLOAD_SIZE(CAMERA_CAPTURE_STATUS); - camera->send_camera_capture_status(chan); + return camera->send_mavlink_message(*this, id); } - break; -#endif +#endif // AP_CAMERA_ENABLED case MSG_SYSTEM_TIME: CHECK_PAYLOAD_SIZE(SYSTEM_TIME);