From 1f47856fbbe56389b75089d9e8df84fe6ef8330d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 8 Nov 2023 14:37:10 +1100 Subject: [PATCH 1/6] AP_EFI: remove unused definitions --- libraries/AP_EFI/AP_EFI_State.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h index 97acbecd6b2209..c73416f5a377c6 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -15,9 +15,6 @@ #pragma once -#define EFI_MAX_INSTANCES 2 -#define EFI_MAX_BACKENDS 2 - #include #include From ba76d4e2f90f99eb4c95790884581e2521ed7c7e Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Tue, 31 Oct 2023 10:04:12 -0500 Subject: [PATCH 2/6] hwdef:save flash on FlywooF405S-AIO --- libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat index de334979da0cd3..3b2beee7d66c43 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat @@ -151,6 +151,6 @@ define HAL_FRAME_TYPE_DEFAULT 12 define DEFAULT_NTF_LED_TYPES 257 # save some flash space -include ../include/no_bootloader_DFU.inc +include ../include/minimize_fpv_osd.inc AUTOBUILD_TARGETS Copter From 477534b44610865ad42c0f17a4cd3f3b7e7de964 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Tue, 12 Sep 2023 12:22:44 +1000 Subject: [PATCH 3/6] AP_Camera: Add handler for CAMERA_CAPTURE_STATUS request --- libraries/AP_Camera/AP_Camera.cpp | 13 +++++++++++++ libraries/AP_Camera/AP_Camera.h | 3 +++ libraries/AP_Camera/AP_Camera_Backend.cpp | 20 ++++++++++++++++++++ libraries/AP_Camera/AP_Camera_Backend.h | 3 +++ 4 files changed, 39 insertions(+) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index b3583a8468df60..a44243c8caccaa 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -541,6 +541,19 @@ void AP_Camera::send_camera_fov_status(mavlink_channel_t chan) } #endif +// send camera capture status message to GCS +void AP_Camera::send_camera_capture_status(mavlink_channel_t chan) +{ + WITH_SEMAPHORE(_rsem); + + // call each instance + for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) { + if (_backends[instance] != nullptr) { + _backends[instance]->send_camera_capture_status(chan); + } + } +} + /* update; triggers by distance moved and camera trigger */ diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index f0f384a1991857..356901ab262816 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -100,6 +100,9 @@ class AP_Camera { 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); + // 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); 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); diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index abda67057dfae8..f6cf2c2d2b6089 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -286,6 +286,26 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const } #endif +// send camera capture status message to GCS +void AP_Camera_Backend::send_camera_capture_status(mavlink_channel_t chan) const +{ + const float NaN = nanf("0x4152"); + + // Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + const uint8_t image_status = (time_interval_settings.num_remaining > 0) ? 2 : 0; + + // send CAMERA_CAPTURE_STATUS message + mavlink_msg_camera_capture_status_send( + chan, + AP_HAL::millis(), + image_status, + 0, // current status of video capturing (0: idle, 1: capture in progress) + static_cast(time_interval_settings.time_interval_ms) / 1000.0, // image capture interval (s) + 0, // elapsed time since recording started (ms) + NaN, // available storage capacity (ms) + image_index); // total number of images captured +} + // setup a callback for a feedback pin. When on PX4 with the right FMU // mode we can use the microsecond timer. void AP_Camera_Backend::setup_feedback_callback() diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index bae4fb8ea291eb..067fa15a080eaa 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -115,6 +115,9 @@ class AP_Camera_Backend void send_camera_fov_status(mavlink_channel_t chan) const; #endif + // send camera capture status message to GCS + virtual void send_camera_capture_status(mavlink_channel_t chan) const; + #if AP_CAMERA_SCRIPTING_ENABLED // accessor to allow scripting backend to retrieve state // returns true on success and cam_state is filled in From ac313b6d7a445f207651f96e5f1ea758dc55cdbf Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Tue, 12 Sep 2023 12:23:17 +1000 Subject: [PATCH 4/6] AP_Mount: Add handler for CAMERA_CAPTURE_STATUS request --- libraries/AP_Mount/AP_Mount.cpp | 10 ++++++++++ libraries/AP_Mount/AP_Mount.h | 3 +++ libraries/AP_Mount/AP_Mount_Backend.h | 3 +++ 3 files changed, 16 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 8ff5a5362ba2d0..9c79d79205d50f 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -850,6 +850,16 @@ void AP_Mount::send_camera_settings(uint8_t instance, mavlink_channel_t chan) co backend->send_camera_settings(chan); } +// send camera capture status message to GCS +void AP_Mount::send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->send_camera_capture_status(chan); +} + // get rangefinder distance. Returns true on success bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) const { diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index fe3003eca536a0..3763a4f8221189 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -256,6 +256,9 @@ class AP_Mount // send camera settings message to GCS void send_camera_settings(uint8_t instance, mavlink_channel_t chan) const; + // send camera capture status message to GCS + void send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const; + // // rangefinder // diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index fbd2ca0a801044..0503d02ae648ab 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -182,6 +182,9 @@ class AP_Mount_Backend // send camera settings message to GCS virtual void send_camera_settings(mavlink_channel_t chan) const {} + // send camera capture status message to GCS + virtual void send_camera_capture_status(mavlink_channel_t chan) const {} + #if AP_MOUNT_POI_TO_LATLONALT_ENABLED // get poi information. Returns true on success and fills in gimbal attitude, location and poi location bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc); From 2f9b9e245262cd709c87e01336543a9046589958 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Tue, 12 Sep 2023 12:23:53 +1000 Subject: [PATCH 5/6] AP_Camera: Add Camera_Mount handler for CAMERA_CAPTURE_STATUS request --- libraries/AP_Camera/AP_Camera_Mount.cpp | 9 +++++++++ libraries/AP_Camera/AP_Camera_Mount.h | 3 +++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index e81327fb58201f..775ac53146af8a 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -88,4 +88,13 @@ void AP_Camera_Mount::send_camera_settings(mavlink_channel_t chan) const } } +// send camera capture status message to GCS +void AP_Camera_Mount::send_camera_capture_status(mavlink_channel_t chan) const +{ + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + return mount->send_camera_capture_status(get_mount_instance(), chan); + } +} + #endif // AP_CAMERA_MOUNT_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index fa53057295b8c2..c453b96cdd6119 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -59,6 +59,9 @@ class AP_Camera_Mount : public AP_Camera_Backend // send camera settings message to GCS void send_camera_settings(mavlink_channel_t chan) const override; + + // send camera capture status message to GCS + void send_camera_capture_status(mavlink_channel_t chan) const override; }; #endif // AP_CAMERA_MOUNT_ENABLED From 3ba63d33d5c3665cff13382d4af154e7300828ff Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Tue, 12 Sep 2023 12:24:21 +1000 Subject: [PATCH 6/6] GCS_MAVLink: Add handler for CAMERA_CAPTURE_STATUS request --- libraries/GCS_MAVLink/GCS_Common.cpp | 11 +++++++++++ libraries/GCS_MAVLink/ap_message.h | 1 + 2 files changed, 12 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fb5615cf3541a7..371f16a8641a74 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -999,6 +999,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_CAMERA_INFORMATION, MSG_CAMERA_INFORMATION}, { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS}, + { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS}, #endif #if HAL_MOUNT_ENABLED { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, @@ -5784,6 +5785,16 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) } break; #endif + case MSG_CAMERA_CAPTURE_STATUS: + { + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + break; + } + CHECK_PAYLOAD_SIZE(CAMERA_CAPTURE_STATUS); + camera->send_camera_capture_status(chan); + } + break; #endif case MSG_SYSTEM_TIME: diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 3a707bc4628d82..973c48d51bad10 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -52,6 +52,7 @@ enum ap_message : uint8_t { MSG_CAMERA_INFORMATION, MSG_CAMERA_SETTINGS, MSG_CAMERA_FOV_STATUS, + MSG_CAMERA_CAPTURE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS,