diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6170ce0096..0a156dd1d8 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -845,6 +845,19 @@ bool AP_Mount::set_lens(uint8_t instance, uint8_t lens) return backend->set_lens(lens); } +#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED +// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type +// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t +bool AP_Mount::set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + return backend->set_camera_source(primary_source, secondary_source); +} +#endif + // send camera information message to GCS void AP_Mount::send_camera_information(uint8_t instance, mavlink_channel_t chan) const { diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 7d761f53f8..0a860d07c5 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -250,6 +250,12 @@ public: // set camera lens as a value from 0 to 5 bool set_lens(uint8_t instance, uint8_t lens); +#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED + // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type + // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t + bool set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source); +#endif + // send camera information message to GCS void send_camera_information(uint8_t instance, mavlink_channel_t chan) const; diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index f64bde9ec6..f21b44851a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -182,6 +182,12 @@ public: // set camera lens as a value from 0 to 5 virtual bool set_lens(uint8_t lens) { return false; } +#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED + // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type + // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t + virtual bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) { return false; } +#endif + // send camera information message to GCS virtual void send_camera_information(mavlink_channel_t chan) const {} diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c33fb3ec94..c8e2ae78d2 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -962,6 +962,64 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens) return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type); } +// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type +// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t +bool AP_Mount_Siyi::set_camera_source(uint8_t primary_source, uint8_t secondary_source) +{ + // only supported on ZT30. sanity check lens values + if (_hardware_model != HardwareModel::ZT30) { + return false; + } + + // maps primary and secondary source to siyi camera image type + CameraImageType cam_image_type; + switch (primary_source) { + case 0: // Default (RGB) + FALLTHROUGH; + case 1: // RGB + switch (secondary_source) { + case 0: // RGB + Default (None) + cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3 + break; + case 2: // PIP RGB+IR + cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0 + break; + case 4: // PIP RGB+RGB_WIDEANGLE + cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2 + break; + default: + return false; + } + break; + case 2: // IR + switch (secondary_source) { + case 0: // IR + Default (None) + cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7 + break; + default: + return false; + } + break; + case 4: // RGB_WIDEANGLE + switch (secondary_source) { + case 0: // RGB_WIDEANGLE + Default (None) + cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5 + break; + case 2: // PIP RGB_WIDEANGLE+IR + cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1 + break; + default: + return false; + } + break; + default: + return false; + } + + // send desired image type to camera + return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type); +} + // send camera information message to GCS void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 2245e4ca0d..6036a8b635 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -75,6 +75,10 @@ public: // set camera lens as a value from 0 to 8. ZT30 only bool set_lens(uint8_t lens) override; + // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type + // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t + bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override; + // send camera information message to GCS void send_camera_information(mavlink_channel_t chan) const override; diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 5e08762348..edce186adf 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -862,6 +862,47 @@ bool AP_Mount_Viewpro::set_lens(uint8_t lens) return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0); } +// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type +// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t +bool AP_Mount_Viewpro::set_camera_source(uint8_t primary_source, uint8_t secondary_source) +{ + // maps primary and secondary source to viewpro image sensor + ImageSensor new_image_sensor; + switch (primary_source) { + case 0: // Default (RGB) + FALLTHROUGH; + case 1: // RGB + switch (secondary_source) { + case 0: // RGB + Default (None) + new_image_sensor = ImageSensor::EO1; + break; + case 2: // PIP RGB+IR + new_image_sensor = ImageSensor::EO1_IR_PIP; + break; + default: + return false; + } + break; + case 2: // IR + switch (secondary_source) { + case 0: // IR + Default (None) + new_image_sensor = ImageSensor::IR; + break; + case 1: // PIP IR+RGB + new_image_sensor = ImageSensor::IR_EO1_PIP; + break; + default: + return false; + } + break; + default: + return false; + } + + // send desired image type to camera + return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0); +} + // send camera information message to GCS void AP_Mount_Viewpro::send_camera_information(mavlink_channel_t chan) const { diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index ed312a2035..552ce6d7b4 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -78,6 +78,10 @@ public: // set camera lens as a value from 0 to 5 bool set_lens(uint8_t lens) override; + // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type + // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t + bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override; + // send camera information message to GCS void send_camera_information(mavlink_channel_t chan) const override; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 239d416455..3d551885ce 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -308,6 +308,48 @@ bool AP_Mount_Xacti::set_lens(uint8_t lens) return set_param_int32(Param::SensorMode, lens); } +// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type +// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t +bool AP_Mount_Xacti::set_camera_source(uint8_t primary_source, uint8_t secondary_source) +{ + // maps primary and secondary source to xacti SensorsMode + SensorsMode new_sensor_mode; + switch (primary_source) { + case 0: // Default (RGB) + FALLTHROUGH; + case 1: // RGB + switch (secondary_source) { + case 0: // RGB + Default (None) + new_sensor_mode = SensorsMode::RGB; + break; + case 2: // PIP RGB+IR + new_sensor_mode = SensorsMode::PIP; + break; + default: + return false; + } + break; + case 2: // IR + if (secondary_source != 0) { + return false; + } + new_sensor_mode = SensorsMode::IR; + break; + case 3: // NDVI + if (secondary_source != 0) { + return false; + } + // NDVI + Default (None) + new_sensor_mode = SensorsMode::NDVI; + break; + default: + return false; + } + + // send desired sensor mode to camera + return set_param_int32(Param::SensorMode, (uint8_t)new_sensor_mode); +} + // send camera information message to GCS void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const { diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 247a66bac1..fc4b20db03 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -62,6 +62,10 @@ public: // set camera lens as a value from 0 to 5 bool set_lens(uint8_t lens) override; + // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type + // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t + bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override; + // send camera information message to GCS void send_camera_information(mavlink_channel_t chan) const override; diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 9159653dcc..6b14bcb480 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -53,3 +53,8 @@ #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED #define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024 #endif + +// set camera source is supported on gimbals that may have more than one lens +#ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED +#define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED +#endif