diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 9a2deb8a18..57302c8bde 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -2,6 +2,7 @@ #if AP_CAMERA_ENABLED +#include #include #include #include @@ -432,6 +433,44 @@ MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet) } } +// send a mavlink message; returns false if there was not space to +// send the message, true otherwise +bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message msg_id) +{ + const auto chan = link.get_chan(); + + switch (msg_id) { + case MSG_CAMERA_FEEDBACK: + CHECK_PAYLOAD_SIZE2(CAMERA_FEEDBACK); + send_feedback(chan); + break; + case MSG_CAMERA_INFORMATION: + CHECK_PAYLOAD_SIZE2(CAMERA_INFORMATION); + send_camera_information(chan); + break; + case MSG_CAMERA_SETTINGS: + CHECK_PAYLOAD_SIZE2(CAMERA_SETTINGS); + send_camera_settings(chan); + break; +#if AP_CAMERA_SEND_FOV_STATUS_ENABLED + case MSG_CAMERA_FOV_STATUS: + CHECK_PAYLOAD_SIZE2(CAMERA_FOV_STATUS); + send_camera_fov_status(chan); + break; +#endif + case MSG_CAMERA_CAPTURE_STATUS: + CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS); + send_camera_capture_status(chan); + break; + + default: + // should not reach this; should only be called for specific IDs + break; + } + + return true; +} + // set camera trigger distance in a mission void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index ee9f142cec..dd9f4ddfde 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -8,6 +8,7 @@ #include #include +#include #include "AP_Camera_Params.h" #include "AP_Camera_shareddefs.h" @@ -86,22 +87,9 @@ public: // handle MAVLink command from GCS to control the camera MAV_RESULT handle_command(const mavlink_command_int_t &packet); - // send camera feedback message to GCS - void send_feedback(mavlink_channel_t chan); - - // send camera information message to GCS - void send_camera_information(mavlink_channel_t chan); - - // send camera settings message to GCS - void send_camera_settings(mavlink_channel_t chan); - -#if AP_CAMERA_SEND_FOV_STATUS_ENABLED - // send camera field of view status - void send_camera_fov_status(mavlink_channel_t chan); -#endif - - // send camera capture status message to GCS - void send_camera_capture_status(mavlink_channel_t chan); + // send a mavlink message; returns false if there was not space to + // send the message, true otherwise + bool send_mavlink_message(class GCS_MAVLINK &link, const enum ap_message id); // configure camera void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time); @@ -232,6 +220,23 @@ private: // perform any required parameter conversion void convert_params(); + // send camera feedback message to GCS + void send_feedback(mavlink_channel_t chan); + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan); + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan); + +#if AP_CAMERA_SEND_FOV_STATUS_ENABLED + // send camera field of view status + void send_camera_fov_status(mavlink_channel_t chan); +#endif + + // send camera capture status message to GCS + void send_camera_capture_status(mavlink_channel_t chan); + HAL_Semaphore _rsem; // semaphore for multi-thread access AP_Camera_Backend *primary; // primary camera backed bool _is_in_auto_mode; // true if in AUTO mode