From 685bdd3d3d190a2fbd62cdacddb6ab3aeb4c4343 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 31 Oct 2023 13:30:01 +1100 Subject: [PATCH] AP_Camera: handle camera messages as COMMAND_INT as well as COMMAND_LONG --- libraries/AP_Camera/AP_Camera.cpp | 14 +++++++------- libraries/AP_Camera/AP_Camera.h | 10 +++++----- libraries/AP_Camera/AP_Camera_Backend.cpp | 4 ++-- libraries/AP_Camera/AP_Camera_Backend.h | 4 ++-- libraries/AP_Camera/AP_Camera_MAVLink.cpp | 14 +++++++------- libraries/AP_Camera/AP_Camera_MAVLink.h | 4 ++-- libraries/AP_Camera/AP_Camera_Servo.cpp | 2 +- libraries/AP_Camera/AP_Camera_Servo.h | 2 +- 8 files changed, 27 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 7be0cb8e83d58a..19864d0023c55e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -279,14 +279,14 @@ void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t & } // handle command_long mavlink messages -MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet) +MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet) { switch (packet.command) { case MAV_CMD_DO_DIGICAM_CONFIGURE: - configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); + configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.x, packet.y, packet.z); return MAV_RESULT_ACCEPTED; case MAV_CMD_DO_DIGICAM_CONTROL: - control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6); + control(packet.param1, packet.param2, packet.param3, packet.param4, packet.x, packet.y); return MAV_RESULT_ACCEPTED; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: set_trigger_distance(packet.param1); @@ -438,7 +438,7 @@ void AP_Camera::cam_mode_toggle(uint8_t instance) } // configure camera -void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) +void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) { WITH_SEMAPHORE(_rsem); @@ -448,7 +448,7 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu primary->configure(shooting_mode, shutter_speed, aperture, ISO, exposure_type, cmd_id, engine_cutoff_time); } -void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) +void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) { WITH_SEMAPHORE(_rsem); @@ -462,7 +462,7 @@ void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_s } // handle camera control -void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) +void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id) { WITH_SEMAPHORE(_rsem); @@ -472,7 +472,7 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo primary->control(session, zoom_pos, zoom_step, focus_lock, shooting_cmd, cmd_id); } -void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) +void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id) { WITH_SEMAPHORE(_rsem); diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 80221ac6434ba7..929e5d512305b5 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -84,7 +84,7 @@ class AP_Camera { void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); // handle MAVLink command from GCS to control the camera - MAV_RESULT handle_command_long(const mavlink_command_long_t &packet); + MAV_RESULT handle_command(const mavlink_command_int_t &packet); // send camera feedback message to GCS void send_feedback(mavlink_channel_t chan); @@ -96,12 +96,12 @@ class AP_Camera { void send_camera_settings(mavlink_channel_t chan); // configure camera - void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time); - void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time); + void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time); + void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time); // handle camera control - void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); - void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); + void control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id); + void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id); // set camera trigger distance in a mission void set_trigger_distance(float distance_m); diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 668053768169a9..738efc75998955 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -161,10 +161,10 @@ void AP_Camera_Backend::stop_capture() } // handle camera control -void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) +void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id) { // take picture - if (is_equal(shooting_cmd, 1.0f)) { + if (shooting_cmd == 1) { take_picture(); } } diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 5f8f8b1d78fb9c..5ae296c57bd432 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -89,10 +89,10 @@ class AP_Camera_Backend virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {} // configure camera - virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) {} + virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) {} // handle camera control - virtual void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id); + virtual void control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id); // set camera trigger distance in meters void set_trigger_distance(float distance_m) { _params.trigg_dist.set(distance_m); } diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.cpp b/libraries/AP_Camera/AP_Camera_MAVLink.cpp index 85b24b2d88314e..3568709f80de41 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLink.cpp +++ b/libraries/AP_Camera/AP_Camera_MAVLink.cpp @@ -19,7 +19,7 @@ bool AP_Camera_MAVLink::trigger_pic() } // configure camera -void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) +void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) { // convert to mavlink message and send to all components mavlink_command_long_t mav_cmd_long = {}; @@ -30,8 +30,8 @@ void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, floa mav_cmd_long.param2 = shutter_speed; mav_cmd_long.param3 = aperture; mav_cmd_long.param4 = ISO; - mav_cmd_long.param5 = exposure_type; - mav_cmd_long.param6 = cmd_id; + mav_cmd_long.param5 = float(exposure_type); + mav_cmd_long.param6 = float(cmd_id); mav_cmd_long.param7 = engine_cutoff_time; // send to all components @@ -39,10 +39,10 @@ void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, floa } // handle camera control message -void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) +void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id) { // take picture and ignore other arguments - if (is_equal(shooting_cmd, 1.0f)) { + if (shooting_cmd == 1) { take_picture(); return; } @@ -54,8 +54,8 @@ void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, mav_cmd_long.param2 = zoom_pos; mav_cmd_long.param3 = zoom_step; mav_cmd_long.param4 = focus_lock; - mav_cmd_long.param5 = shooting_cmd; - mav_cmd_long.param6 = cmd_id; + mav_cmd_long.param5 = float(shooting_cmd); + mav_cmd_long.param6 = float(cmd_id); // send to all components GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long)); diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.h b/libraries/AP_Camera/AP_Camera_MAVLink.h index 0be92f09c0729c..01840b6389cd80 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLink.h +++ b/libraries/AP_Camera/AP_Camera_MAVLink.h @@ -36,10 +36,10 @@ class AP_Camera_MAVLink : public AP_Camera_Backend bool trigger_pic() override; // configure camera - void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override; + void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) override; // handle camera control message - void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) override; + void control(float session, float zoom_pos, float zoom_step, float focus_lock, uint32_t shooting_cmd, uint32_t cmd_id) override; }; #endif // AP_CAMERA_MAVLINK_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Servo.cpp b/libraries/AP_Camera/AP_Camera_Servo.cpp index b297576d0347f2..30dd9c385ae3ac 100644 --- a/libraries/AP_Camera/AP_Camera_Servo.cpp +++ b/libraries/AP_Camera/AP_Camera_Servo.cpp @@ -44,7 +44,7 @@ bool AP_Camera_Servo::trigger_pic() } // configure camera -void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) +void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) { // designed to control Blackmagic Micro Cinema Camera (BMMCC) cameras // if the message contains non zero values then use them for the below functions diff --git a/libraries/AP_Camera/AP_Camera_Servo.h b/libraries/AP_Camera/AP_Camera_Servo.h index bd2a585b8e0d29..e9edb40387add1 100644 --- a/libraries/AP_Camera/AP_Camera_Servo.h +++ b/libraries/AP_Camera/AP_Camera_Servo.h @@ -39,7 +39,7 @@ class AP_Camera_Servo : public AP_Camera_Backend bool trigger_pic() override; // configure camera - void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override; + void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, uint32_t exposure_type, uint32_t cmd_id, float engine_cutoff_time) override; private: