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

AP_Camera: support camera info and settings

This commit is contained in:
Randy Mackay 2023-06-10 11:17:12 +09:00
parent 2c37958c3f
commit 63b4b8b7d3
9 changed files with 153 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Camera/AP_Camera.cpp
View File

@ -376,6 +376,32 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
}
}
// send camera information message to GCS
void AP_Camera::send_camera_information(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_information(chan);
}
}
}
// send camera settings message to GCS
void AP_Camera::send_camera_settings(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_settings(chan);
}
}
}
/*
update; triggers by distance moved and camera trigger
*/

6
libraries/AP_Camera/AP_Camera.h
View File

@ -89,6 +89,12 @@ public:
// send camera feedback message to GCS
void send_feedback(mavlink_channel_t chan);
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan);
// send camera settings message to GCS
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);

42
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -141,6 +141,48 @@ void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
camera_feedback.feedback_trigger_logged_count); // completed image captures
}
// send camera information message to GCS
void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
{
// prepare vendor, model and cam definition strings
const uint8_t vendor_name[32] {};
const uint8_t model_name[32] {};
const char cam_definition_uri[140] {};
const uint32_t cap_flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
const float NaN = nanf("0x4152");
// send CAMERA_INFORMATION message
mavlink_msg_camera_information_send(
chan,
AP_HAL::millis(), // time_boot_ms
vendor_name, // vendor_name uint8_t[32]
model_name, // model_name uint8_t[32]
0, // firmware version uint32_t
NaN, // focal_length float (mm)
NaN, // sensor_size_h float (mm)
NaN, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id, uint8_t
cap_flags, // flags uint32_t (CAMERA_CAP_FLAGS)
0, // cam_definition_version uint16_t
cam_definition_uri); // cam_definition_uri char[140]
}
// send camera settings message to GCS
void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
{
const float NaN = nanf("0x4152");
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
NaN, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
// 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()

6
libraries/AP_Camera/AP_Camera_Backend.h
View File

@ -85,6 +85,12 @@ public:
// send camera feedback message to GCS
void send_camera_feedback(mavlink_channel_t chan);
// send camera information message to GCS
virtual void send_camera_information(mavlink_channel_t chan) const;
// send camera settings message to GCS
virtual void send_camera_settings(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

52
libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp
View File

@ -23,7 +23,7 @@ void AP_Camera_MAVLinkCamV2::update()
bool AP_Camera_MAVLinkCamV2::trigger_pic()
{
// exit immediately if have not found camera or does not support taking pictures
if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
return false;
}
@ -43,7 +43,7 @@ bool AP_Camera_MAVLinkCamV2::trigger_pic()
bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording)
{
// exit immediately if have not found camera or does not support recording video
if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
return false;
}
@ -68,7 +68,7 @@ bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording)
bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
{
// exit immediately if have not found camera or does not support zoom
if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
return false;
}
@ -95,7 +95,7 @@ bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
bool AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
{
// exit immediately if have not found camera or does not support focus
if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
return false;
}
@ -133,30 +133,52 @@ void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlin
// handle CAMERA_INFORMATION
if (msg.msgid == MAVLINK_MSG_ID_CAMERA_INFORMATION) {
mavlink_camera_information_t cam_info;
mavlink_msg_camera_information_decode(&msg, &cam_info);
mavlink_msg_camera_information_decode(&msg, &_cam_info);
const uint8_t fw_ver_major = cam_info.firmware_version & 0x000000FF;
const uint8_t fw_ver_minor = (cam_info.firmware_version & 0x0000FF00) >> 8;
const uint8_t fw_ver_revision = (cam_info.firmware_version & 0x00FF0000) >> 16;
const uint8_t fw_ver_build = (cam_info.firmware_version & 0xFF000000) >> 24;
const uint8_t fw_ver_major = _cam_info.firmware_version & 0x000000FF;
const uint8_t fw_ver_minor = (_cam_info.firmware_version & 0x0000FF00) >> 8;
const uint8_t fw_ver_revision = (_cam_info.firmware_version & 0x00FF0000) >> 16;
const uint8_t fw_ver_build = (_cam_info.firmware_version & 0xFF000000) >> 24;
// display camera info to user
gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u",
cam_info.vendor_name,
cam_info.model_name,
_cam_info.vendor_name,
_cam_info.model_name,
(unsigned)fw_ver_major,
(unsigned)fw_ver_minor,
(unsigned)fw_ver_revision,
(unsigned)fw_ver_build);
// capability flags
_cap_flags = cam_info.flags;
_got_camera_info = true;
}
}
// send camera information message to GCS
void AP_Camera_MAVLinkCamV2::send_camera_information(mavlink_channel_t chan) const
{
// exit immediately if we have not yet received cam info
if (!_got_camera_info) {
return;
}
// send CAMERA_INFORMATION message
mavlink_msg_camera_information_send(
chan,
AP_HAL::millis(), // time_boot_ms
_cam_info.vendor_name, // vendor_name uint8_t[32]
_cam_info.model_name, // model_name uint8_t[32]
_cam_info.firmware_version, // firmware version uint32_t
_cam_info.focal_length, // focal_length float (mm)
_cam_info.sensor_size_h, // sensor_size_h float (mm)
_cam_info.sensor_size_v, // sensor_size_v float (mm)
_cam_info.resolution_h, // resolution_h uint16_t (pix)
_cam_info.resolution_v, // resolution_v uint16_t (pix)
_cam_info.lens_id, // lens_id, uint8_t
_cam_info.flags, // flags uint32_t (CAMERA_CAP_FLAGS)
_cam_info.cam_definition_version, // cam_definition_version uint16_t
_cam_info.cam_definition_uri); // cam_definition_uri char[140]
}
// search for camera in GCS_MAVLink routing table
void AP_Camera_MAVLinkCamV2::find_camera()
{

5
libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h
View File

@ -53,6 +53,9 @@ public:
// handle MAVLink messages from the camera
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override;
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;
private:
// search for camera in GCS_MAVLink routing table
@ -64,11 +67,11 @@ private:
// internal members
bool _initialised; // true once the camera has provided a CAMERA_INFORMATION
bool _got_camera_info; // true once camera has provided CAMERA_INFORMATION
mavlink_camera_information_t _cam_info {}; // latest camera information received from camera
uint32_t _last_caminfo_req_ms; // system time that CAMERA_INFORMATION was last requested (used to throttle requests)
class GCS_MAVLINK *_link; // link we have found the camera on. nullptr if not seen yet
uint8_t _sysid; // sysid of camera
uint8_t _compid; // component id of gimbal
uint32_t _cap_flags; // capability flags from CAMERA_INFORMATION msg, see MAVLink CAMERA_CAP_FLAGS enum
};
#endif // AP_CAMERA_MAVLINKCAMV2_ENABLED

18
libraries/AP_Camera/AP_Camera_Mount.cpp
View File

@ -48,4 +48,22 @@ bool AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
return false;
}
// send camera information message to GCS
void AP_Camera_Mount::send_camera_information(mavlink_channel_t chan) const
{
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->send_camera_information(chan);
}
}
// send camera settings message to GCS
void AP_Camera_Mount::send_camera_settings(mavlink_channel_t chan) const
{
AP_Mount* mount = AP::mount();
if (mount != nullptr) {
return mount->send_camera_settings(chan);
}
}
#endif // AP_CAMERA_MOUNT_ENABLED

6
libraries/AP_Camera/AP_Camera_Mount.h
View File

@ -45,6 +45,12 @@ public:
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
bool set_focus(FocusType focus_type, float focus_value) override;
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;
// send camera settings message to GCS
void send_camera_settings(mavlink_channel_t chan) const override;
};
#endif // AP_CAMERA_MOUNT_ENABLED

14
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -311,9 +311,11 @@ void AP_Mount_Siyi::process_packet()
_hardware_model = (_parsed_msg.data_bytes_received <= 8) ? HardwareModel::A8 : HardwareModel::ZR10;
// consume and display camera firmware version
_cam_firmware_version = {_msg_buff[_msg_buff_data_start+2], // firmware major version
_msg_buff[_msg_buff_data_start+1], // firmware minor version
_msg_buff[_msg_buff_data_start+0]}; // firmware revision (aka patch)
_cam_firmware_version = {
_msg_buff[_msg_buff_data_start+2], // firmware major version
_msg_buff[_msg_buff_data_start+1], // firmware minor version
_msg_buff[_msg_buff_data_start+0] // firmware revision (aka patch)
};
debug("Mount: SiyiCam fw:%u.%u.%u",
(unsigned)_cam_firmware_version.major, // firmware major version
@ -791,9 +793,9 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
return;
}
const uint8_t vendor_name[32] = "Siyi";
uint8_t model_name[32] = "Unknown";
uint32_t fw_version = _cam_firmware_version.major | (_cam_firmware_version.minor << 8) | (_cam_firmware_version.patch << 16);
static const uint8_t vendor_name[32] = "Siyi";
static uint8_t model_name[32] = "Unknown";
const uint32_t fw_version = _cam_firmware_version.major | (_cam_firmware_version.minor << 8) | (_cam_firmware_version.patch << 16);
const char cam_definition_uri[140] {};
// focal length