diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 2da688ab12410..b61950f1248fb 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -5620,9 +5620,9 @@ def SendToComponents(self): 0, 0) - self.context_collect('COMMAND_LONG') - self.progress("Sending control message") + self.context_push() + self.context_collect('COMMAND_LONG') self.mav.mav.digicam_control_send( 1, # target_system 1, # target_component @@ -5641,6 +5641,52 @@ def SendToComponents(self): 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, 'param6': 17, }, timeout=2, check_context=True) + self.context_pop() + + # test sending via commands: + for run_cmd in self.run_cmd, self.run_cmd_int: + self.progress("Sending control command") + self.context_push() + self.context_collect('COMMAND_LONG') + run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + p1=1, # start or keep it up + p2=1, # zoom_pos + p3=0, # zoom_step + p4=0, # focus_lock + p5=0, # 1 shot or start filming + p6=37, # command id (de-dupe field) + ) + + self.assert_received_message_field_values('COMMAND_LONG', { + 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + 'param6': 37, + }, timeout=2, check_context=True) + + self.context_pop() + + # test sending via commands: + for run_cmd in self.run_cmd, self.run_cmd_int: + self.progress("Sending configure command") + self.context_push() + self.context_collect('COMMAND_LONG') + run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE, + p1=1, + p2=1, + p3=0, + p4=0, + p5=12, + p6=37 + ) + + self.assert_received_message_field_values('COMMAND_LONG', { + 'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE, + 'param5': 12, + 'param6': 37, + }, timeout=2, check_context=True) + + self.context_pop() + + self.mav.mav.srcSystem = old_srcSystem def SkidSteer(self): '''Check skid-steering''' diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 7be0cb8e83d58..d8f4d67ba9928 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, int32_t exposure_type, int32_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, int32_t exposure_type, int32_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, int32_t shooting_cmd, int32_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, int32_t shooting_cmd, int32_t cmd_id) { WITH_SEMAPHORE(_rsem); diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 80221ac6434ba..245bdd60bb130 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, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time); + void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_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, int32_t shooting_cmd, int32_t cmd_id); + void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_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 668053768169a..c936f2127dea5 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, int32_t shooting_cmd, int32_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 5f8f8b1d78fb9..aabe710299473 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, int32_t exposure_type, int32_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, int32_t shooting_cmd, int32_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 85b24b2d88314..b5bebb91a0944 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, int32_t exposure_type, int32_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, int32_t shooting_cmd, int32_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 0be92f09c0729..396b31f7ac1f3 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, int32_t exposure_type, int32_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, int32_t shooting_cmd, int32_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 b297576d0347f..80c3d7d7ba41f 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, int32_t exposure_type, int32_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 bd2a585b8e0d2..7a9afa25d3cb3 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, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) override; private: diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index a505f760b9c49..7ba2c0c2c8b46 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -654,7 +654,7 @@ class GCS_MAVLINK virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const; MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg); - MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc); MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d5c407ed2fa87..e33ccbc71c766 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3449,14 +3449,14 @@ void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg) #endif #if AP_CAMERA_ENABLED -MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_int_t &packet) { AP_Camera *camera = AP::camera(); if (camera == nullptr) { return MAV_RESULT_UNSUPPORTED; } - return camera->handle_command_long(packet); + return camera->handle_command(packet); } #endif @@ -4724,22 +4724,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t switch (packet.command) { -#if AP_CAMERA_ENABLED - case MAV_CMD_DO_DIGICAM_CONFIGURE: - case MAV_CMD_DO_DIGICAM_CONTROL: - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - case MAV_CMD_SET_CAMERA_ZOOM: - case MAV_CMD_SET_CAMERA_FOCUS: - case MAV_CMD_IMAGE_START_CAPTURE: - case MAV_CMD_IMAGE_STOP_CAPTURE: - case MAV_CMD_CAMERA_TRACK_POINT: - case MAV_CMD_CAMERA_TRACK_RECTANGLE: - case MAV_CMD_CAMERA_STOP_TRACKING: - case MAV_CMD_VIDEO_START_CAPTURE: - case MAV_CMD_VIDEO_STOP_CAPTURE: - result = handle_command_camera(packet); - break; -#endif #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: result = handle_command_do_gripper(packet); @@ -5036,6 +5020,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_storage_format(const mavlink_command_int_ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { + #if HAL_INS_ACCELCAL_ENABLED case MAV_CMD_ACCELCAL_VEHICLE_POS: return handle_command_accelcal_vehicle_pos(packet); @@ -5072,6 +5057,22 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_DO_SET_MODE: return handle_command_do_set_mode(packet); +#if AP_CAMERA_ENABLED + case MAV_CMD_DO_DIGICAM_CONFIGURE: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_SET_CAMERA_ZOOM: + case MAV_CMD_SET_CAMERA_FOCUS: + case MAV_CMD_IMAGE_START_CAPTURE: + case MAV_CMD_IMAGE_STOP_CAPTURE: + case MAV_CMD_CAMERA_TRACK_POINT: + case MAV_CMD_CAMERA_TRACK_RECTANGLE: + case MAV_CMD_CAMERA_STOP_TRACKING: + case MAV_CMD_VIDEO_START_CAPTURE: + case MAV_CMD_VIDEO_STOP_CAPTURE: + return handle_command_camera(packet); +#endif + case MAV_CMD_DO_SET_ROI_NONE: { const Location zero_loc = Location(); return handle_command_do_set_roi(zero_loc);