Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: add set_camera_source support

This commit is contained in:
Randy Mackay 2024-03-18 10:51:00 +09:00
parent 5a22fb8da5
commit a6a5cde34c
10 changed files with 183 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Mount/AP_Mount.cpp
View File

@ -833,6 +833,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
{

6
libraries/AP_Mount/AP_Mount.h
View File

@ -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;

6
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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 {}

58
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -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
{

4
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -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;

41
libraries/AP_Mount/AP_Mount_Viewpro.cpp
View File

@ -863,6 +863,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
{

4
libraries/AP_Mount/AP_Mount_Viewpro.h
View File

@ -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;

42
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -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
{

4
libraries/AP_Mount/AP_Mount_Xacti.h
View File

@ -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;

5
libraries/AP_Mount/AP_Mount_config.h
View File

@ -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