Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: xacti supports set lens and zoom

Browse Source
This commit is contained in:
Randy Mackay 2023-08-02 15:34:34 +09:00 committed by Andrew Tridgell
parent 1769453520
commit 820d00dfcd
2 changed files with 191 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

169
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -14,6 +14,11 @@ extern const AP_HAL::HAL& hal;
#define XACTI_PARAM_SINGLESHOT "SingleShot"
#define XACTI_PARAM_RECORDING "Recording"
#define XACTI_PARAM_FOCUSMODE "FocusMode"
#define XACTI_PARAM_SENSORMODE "SensorMode"
#define XACTI_PARAM_DIGITALZOOM "DigitalZoomMagnification"
#define XACTI_MSG_SEND_MIN_MS 20 // messages should not be sent to camera more often than 20ms
#define XACTI_ZOOM_RATE_UPDATE_INTERVAL_MS 500 // zoom rate control increments zoom by 10% up or down every 0.5sec
#define AP_MOUNT_XACTI_DEBUG 0
#define debug(fmt, args ...) do { if (AP_MOUNT_XACTI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Xacti: " fmt, ## args); } } while (0)
@ -21,6 +26,8 @@ extern const AP_HAL::HAL& hal;
bool AP_Mount_Xacti::_subscribed = false;
AP_Mount_Xacti::DetectedModules AP_Mount_Xacti::_detected_modules[];
HAL_Semaphore AP_Mount_Xacti::_sem_registry;
const char* AP_Mount_Xacti::send_text_prefix = "Xacti:";
const char* AP_Mount_Xacti::sensor_mode_str[] = { "RGB", "IR", "PIP", "NDVI" };
// Constructor
AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance) :
@ -46,8 +53,22 @@ void AP_Mount_Xacti::update()
return;
}
// return immediately if any message sent is unlikely to be processed
if (!is_safe_to_send()) {
return;
}
// periodically send copter attitude and GPS status
send_copter_att_status();
if (send_copter_att_status()) {
// if message sent avoid sending other messages
return;
}
// update zoom rate control
if (update_zoom_rate_control()) {
// if message sent avoid sending other messages
return;
}
// update based on mount mode
switch (get_mode()) {
@ -150,29 +171,49 @@ bool AP_Mount_Xacti::take_picture()
}
// set SingleShot parameter
return _detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, XACTI_PARAM_SINGLESHOT, 0, &param_int_cb);
return set_param_int32(XACTI_PARAM_SINGLESHOT, 0);
}
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
bool AP_Mount_Xacti::record_video(bool start_recording)
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return false;
return set_param_int32(XACTI_PARAM_RECORDING, start_recording ? 1 : 0);
}
// set zoom specified as a rate or percentage
bool AP_Mount_Xacti::set_zoom(ZoomType zoom_type, float zoom_value)
{
// zoom rate
if (zoom_type == ZoomType::RATE) {
if (is_zero(zoom_value)) {
// stop zooming
_zoom_rate_control.enabled = false;
} else {
// zoom in or out
_zoom_rate_control.enabled = true;
_zoom_rate_control.increment = (zoom_value < 0) ? -100 : 100;
}
return true;
}
// set Recording parameter
return _detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, XACTI_PARAM_RECORDING, start_recording ? 1 : 0, &param_int_cb);
// zoom percentage
if (zoom_type == ZoomType::PCT) {
// convert zoom percentage (0 ~ 100) to zoom parameter value (100, 200, 300, ... 1000)
// 0~9pct:100, 10~19pct:200, ... 90~100pct:1000
uint16_t zoom_param_value = constrain_uint16(uint16_t(zoom_value * 0.1) * 10, 100, 1000);
return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_param_value);
}
// unsupported zoom type
return false;
}
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult AP_Mount_Xacti::set_focus(FocusType focus_type, float focus_value)
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return SetFocusResult::FAILED;
}
// convert focus type and value to parameter value
uint8_t focus_param_value;
switch (focus_type) {
@ -192,10 +233,18 @@ SetFocusResult AP_Mount_Xacti::set_focus(FocusType focus_type, float focus_value
}
// set FocusMode parameter
if (!_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, XACTI_PARAM_FOCUSMODE, focus_param_value, &param_int_cb)) {
return SetFocusResult::FAILED;
return set_param_int32(XACTI_PARAM_FOCUSMODE, focus_param_value) ? SetFocusResult::ACCEPTED : SetFocusResult::FAILED;
}
// set camera lens as a value from 0 to 5
bool AP_Mount_Xacti::set_lens(uint8_t lens)
{
// sanity check
if (lens > (uint8_t)SensorsMode::NDVI) {
return false;
}
return SetFocusResult::ACCEPTED;
return set_param_int32(XACTI_PARAM_SENSORMODE, lens);
}
// send camera information message to GCS
@ -445,6 +494,24 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
}
return false;
}
if (strcmp(name, XACTI_PARAM_SENSORMODE) == 0) {
if (value < 0) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s change lens", err_prefix_str);
} else if ((uint32_t)value < ARRAY_SIZE(sensor_mode_str)) {
gcs().send_text(MAV_SEVERITY_INFO, "Xacti: %s", sensor_mode_str[(uint8_t)value]);
}
return false;
}
if (strcmp(name, XACTI_PARAM_DIGITALZOOM) == 0) {
if (value < 0) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s change zoom", err_prefix_str);
// disable zoom rate control (if active) to avoid repeated failures
_zoom_rate_control.enabled = false;
} else if (value >= 100 && value <= 1000) {
_last_zoom_param_value = value;
}
return false;
}
// unhandled parameter get or set
gcs().send_text(MAV_SEVERITY_INFO, "Xacti: get/set %s res:%ld", name, (long int)value);
return false;
@ -458,6 +525,20 @@ void AP_Mount_Xacti::handle_param_save_response(AP_DroneCAN* ap_dronecan, const
}
}
// helper function to set integer parameters
bool AP_Mount_Xacti::set_param_int32(const char* param_name, int32_t param_value)
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return false;
}
if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name, param_value, &param_int_cb)) {
last_send_set_param_ms = AP_HAL::millis();
return true;
}
return false;
}
// send gimbal control message via DroneCAN
// mode is 2:angle control or 3:rate control
// pitch_cd is pitch angle in centi-degrees or pitch rate in cds
@ -486,23 +567,24 @@ void AP_Mount_Xacti::send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t
}
// send copter attitude status message to gimbal
void AP_Mount_Xacti::send_copter_att_status()
// returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::send_copter_att_status()
{
// exit immediately if no DroneCAN port
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return;
return false;
}
// send at no faster than 5hz
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_send_copter_att_status_ms < 100) {
return;
return false;
}
// send xacti specific vehicle attitude message
Quaternion veh_att;
if (!AP::ahrs().get_quaternion(veh_att)) {
return;
return false;
}
last_send_copter_att_status_ms = now_ms;
@ -515,6 +597,59 @@ void AP_Mount_Xacti::send_copter_att_status()
copter_att_status_msg.reserved.data[0] = 0;
copter_att_status_msg.reserved.data[1] = 0;
_detected_modules[_instance].ap_dronecan->xacti_copter_att_status.broadcast(copter_att_status_msg);
return true;
}
// update zoom rate controller
// returns true if sent so that we avoid immediately trying to also send other messages
bool AP_Mount_Xacti::update_zoom_rate_control()
{
// return immediately if zoom rate control is not enabled
if (!_zoom_rate_control.enabled) {
return false;
}
// update only every 0.5 sec
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - _zoom_rate_control.last_update_ms < XACTI_ZOOM_RATE_UPDATE_INTERVAL_MS) {
return false;
}
_zoom_rate_control.last_update_ms = now_ms;
// increment zoom
const uint16_t zoom_value = _last_zoom_param_value + _zoom_rate_control.increment;
// if reached limit then disable zoom
if ((zoom_value < 100) || (zoom_value > 1000)) {
_zoom_rate_control.enabled = false;
return false;
}
// send desired zoom to camera
return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_value);
}
// check if safe to send message (if messages sent too often camera will not respond)
bool AP_Mount_Xacti::is_safe_to_send() const
{
const uint32_t now_ms = AP_HAL::millis();
// check time since last attitude sent
if (now_ms - last_send_copter_att_status_ms < XACTI_MSG_SEND_MIN_MS) {
return false;
}
// check time since last angle target sent
if (now_ms - last_send_gimbal_control_ms < XACTI_MSG_SEND_MIN_MS) {
return false;
}
// check time since last set param message sent
if (now_ms - last_send_set_param_ms < XACTI_MSG_SEND_MIN_MS) {
return false;
}
return true;
}
#endif // HAL_MOUNT_XACTI_ENABLED

40
libraries/AP_Mount/AP_Mount_Xacti.h
View File

@ -51,10 +51,16 @@ public:
// set start_recording = true to start record, false to stop recording
bool record_video(bool start_recording) override;
// set zoom specified as a rate or percentage
bool set_zoom(ZoomType zoom_type, float zoom_value) override;
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
// set camera lens as a value from 0 to 5
bool set_lens(uint8_t lens) override;
// send camera information message to GCS
void send_camera_information(mavlink_channel_t chan) const override;
@ -75,6 +81,20 @@ protected:
private:
// send text prefix string to reduce flash cost
static const char* send_text_prefix;
// Sensor mode enumeration (aka lens)
enum class SensorsMode : uint8_t {
RGB = 0, // RGB (aka "visible)")
IR = 1, // Infrared, aka thermal
PIP = 2, // RGB with IR PIP
NDVI = 3, // NDVI (vegetation greenness)
};
// array of sensor mode enumeration strings for display to user
// if enum above is updated, also update sensor_mode_str definition in cpp
static const char* sensor_mode_str[];
// send target pitch and yaw rates to gimbal
// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);
@ -96,6 +116,9 @@ private:
bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
// helper function to set integer parameters
bool set_param_int32(const char* param_name, int32_t param_value);
// send gimbal control message via DroneCAN
// mode is 2:angle control or 3:rate control
// pitch_cd is pitch angle in centi-degrees or pitch rate in cds
@ -103,7 +126,15 @@ private:
void send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd);
// send vehicle attitude to gimbal via DroneCAN
void send_copter_att_status();
// returns true if sent so that we avoid immediately trying to also send other messages
bool send_copter_att_status();
// update zoom rate controller
// returns true if sent so that we avoid immediately trying to also send other messages
bool update_zoom_rate_control();
// check if safe to send message (if messages sent too often camera will not respond)
bool is_safe_to_send() const;
// internal variables
bool _initialised; // true once the driver has been initialised
@ -112,6 +143,12 @@ private:
Quaternion _current_attitude_quat; // current attitude as a quaternion
uint32_t _last_current_attitude_quat_ms; // system time _current_angle_rad was updated
bool _recording_video; // true if recording video
uint16_t _last_zoom_param_value = 100; // last digital zoom parameter value sent to camera. 100 ~ 1000 (interval 100)
struct {
bool enabled; // true if zoom rate control is enabled
int8_t increment; // zoom increment on each update (+100 or -100)
uint32_t last_update_ms; // system time that zoom rate control last updated zoom
} _zoom_rate_control;
// DroneCAN related variables
static bool _subscribed; // true once subscribed to receive DroneCAN messages
@ -123,6 +160,7 @@ private:
static HAL_Semaphore _sem_registry; // semaphore protecting access to _detected_modules table
uint32_t last_send_gimbal_control_ms; // system time that send_gimbal_control was last called (used to slow down sends to 5hz)
uint32_t last_send_copter_att_status_ms; // system time that send_copter_att_status was last called (used to slow down sends to 10hz)
uint32_t last_send_set_param_ms; // system time that a set parameter message was sent
};
#endif // HAL_MOUNT_XACTI_ENABLED