Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: Add capability to set CAMERA_INFORMATION from Lua

Browse Source
This commit is contained in:
Nick Exton 2024-08-08 16:02:42 +10:00 committed by Randy Mackay
parent 0f3667613e
commit 30a212b4cf
5 changed files with 87 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Camera/AP_Camera.cpp
View File

@ -816,6 +816,32 @@ bool AP_Camera::change_setting(uint8_t instance, CameraSetting setting, float va
#endif // #if AP_CAMERA_SCRIPTING_ENABLED
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
void AP_Camera::set_camera_information(mavlink_camera_information_t camera_info)
{
WITH_SEMAPHORE(_rsem);
if (primary == nullptr) {
return;
}
return primary->set_camera_information(camera_info);
}
void AP_Camera::set_camera_information(uint8_t instance, mavlink_camera_information_t camera_info)
{
WITH_SEMAPHORE(_rsem);
auto *backend = get_instance(instance);
if (backend == nullptr) {
return;
}
// call instance
backend->set_camera_information(camera_info);
}
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// return backend for instance number
AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const
{

5
libraries/AP_Camera/AP_Camera.h
View File

@ -186,6 +186,11 @@ public:
bool change_setting(uint8_t instance, CameraSetting setting, float value);
#endif
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
void set_camera_information(mavlink_camera_information_t camera_info);
void set_camera_information(uint8_t instance, mavlink_camera_information_t camera_info);
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// Return true and the relay index if relay camera backend is selected, used for conversion to relay functions
bool get_legacy_relay_index(int8_t &index) const;

65
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -18,6 +18,17 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &para
_instance(instance)
{}
void AP_Camera_Backend::init()
{
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
_camera_info.focal_length = NaNf;
_camera_info.sensor_size_h = NaNf;
_camera_info.sensor_size_v = NaNf;
_camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
}
// update - should be called at 50hz
void AP_Camera_Backend::update()
{
@ -212,31 +223,41 @@ void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
// 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;
mavlink_camera_information_t camera_info;
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// 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
NaNf, // focal_length float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // 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]
get_gimbal_device_id());// gimbal_device_id uint8_t
camera_info = _camera_info;
#else
memset(&camera_info, 0, sizeof(camera_info));
camera_info.focal_length = NaNf;
camera_info.sensor_size_h = NaNf;
camera_info.sensor_size_v = NaNf;
camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// Set fixed fields
// lens_id is populated with the instance number, to disambiguate multiple cameras
camera_info.lens_id = _instance;
camera_info.gimbal_device_id = get_gimbal_device_id();
camera_info.time_boot_ms = AP_HAL::millis();
// Send CAMERA_INFORMATION message
mavlink_msg_camera_information_send_struct(chan, &camera_info);
}
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
void AP_Camera_Backend::set_camera_information(mavlink_camera_information_t camera_info)
{
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u CAMERA_INFORMATION (%s %s) set from script", _instance, camera_info.vendor_name, camera_info.model_name);
_camera_info = camera_info;
};
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// send camera settings message to GCS
void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
{

10
libraries/AP_Camera/AP_Camera_Backend.h
View File

@ -45,7 +45,7 @@ public:
}
// init - performs any required initialisation
virtual void init() {};
virtual void init();
// update - should be called at 50hz
virtual void update();
@ -115,6 +115,10 @@ public:
// send camera information message to GCS
virtual void send_camera_information(mavlink_channel_t chan) const;
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
void set_camera_information(mavlink_camera_information_t camera_info);
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// send camera settings message to GCS
virtual void send_camera_settings(mavlink_channel_t chan) const;
@ -181,6 +185,10 @@ protected:
// get mavlink gimbal device id which is normally mount_instance+1
uint8_t get_gimbal_device_id() const;
#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
mavlink_camera_information_t _camera_info;
#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
// internal members
uint8_t _instance; // this instance's number
bool timer_installed; // true if feedback pin change detected using timer

4
libraries/AP_Camera/AP_Camera_config.h
View File

@ -58,3 +58,7 @@
#ifndef HAL_RUNCAM_ENABLED
#define HAL_RUNCAM_ENABLED 1
#endif
#ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
#define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED
#endif