Skip to content

Commit

Permalink
AP_Mount: Xacti take pic reliability improved
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Sep 23, 2023
1 parent fef1968 commit 67c160a
Show file tree
Hide file tree
Showing 2 changed files with 128 additions and 42 deletions.
135 changes: 96 additions & 39 deletions libraries/AP_Mount/AP_Mount_Xacti.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,18 +12,10 @@
extern const AP_HAL::HAL& hal;

#define LOG_TAG "Mount"
#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_PARAM_FIRMWAREVERSION "FirmwareVersion"
#define XACTI_PARAM_STATUS "Status"
#define XACTI_PARAM_DATETIME "DateTime"

#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 XACTI_STATUS_REQ_INTERVAL_MS 3000 // request status every 3 seconds
#define XACTI_SET_PARAM_QUEUE_SIZE 3 // three set-param requests may be queued

#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)
Expand All @@ -33,6 +25,7 @@ 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" };
const char* AP_Mount_Xacti::_param_names[] = {"SingleShot", "Recording", "FocusMode", "SensorMode", "DigitalZoomMagnification", "FirmwareVersion", "Status", "DateTime"};

// Constructor
AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance) :
Expand All @@ -43,11 +36,20 @@ AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params &
param_int_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_get_set_response_int, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
param_string_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_get_set_response_string, bool, AP_DroneCAN*, const uint8_t, const char*, AP_DroneCAN::string &);
param_save_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_save_response, void, AP_DroneCAN*, const uint8_t, bool);

// static assert that Param enum matches parameter names array
static_assert((uint8_t)AP_Mount_Xacti::Param::LAST+1 == ARRAY_SIZE(AP_Mount_Xacti::_param_names), "AP_Mount_Xacti::_param_names array must match Param enum");
}

// init - performs any required initialisation for this instance
void AP_Mount_Xacti::init()
{
// instantiate parameter queue, return on failure so init fails
_set_param_int32_queue = new ObjectArray<SetParamQueueItem>(XACTI_SET_PARAM_QUEUE_SIZE);
if (_set_param_int32_queue == nullptr) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s init failed", send_text_prefix);
return;
}
_initialised = true;
}

Expand Down Expand Up @@ -80,6 +82,11 @@ void AP_Mount_Xacti::update()
return;
}

// process queue of set parameter items
if (process_set_param_int32_queue()) {
return;
}

// periodically send copter attitude and GPS status
if (send_copter_att_status(now_ms)) {
// if message sent avoid sending other messages
Expand Down Expand Up @@ -188,19 +195,20 @@ bool AP_Mount_Xacti::healthy() const
// take a picture. returns true on success
bool AP_Mount_Xacti::take_picture()
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
// fail if camera errored or already taking picture
if (_camera_error) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s unable to take pic", send_text_prefix);
return false;
}

// set SingleShot parameter
return set_param_int32(XACTI_PARAM_SINGLESHOT, 0);
return set_param_int32(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)
{
return set_param_int32(XACTI_PARAM_RECORDING, start_recording ? 1 : 0);
return set_param_int32(Param::Recording, start_recording ? 1 : 0);
}

// set zoom specified as a rate or percentage
Expand All @@ -224,7 +232,7 @@ bool AP_Mount_Xacti::set_zoom(ZoomType zoom_type, float zoom_value)
// 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);
return set_param_int32(Param::DigitalZoomMagnification, zoom_param_value);
}

// unsupported zoom type
Expand Down Expand Up @@ -254,7 +262,7 @@ SetFocusResult AP_Mount_Xacti::set_focus(FocusType focus_type, float focus_value
}

// set FocusMode parameter
return set_param_int32(XACTI_PARAM_FOCUSMODE, focus_param_value) ? SetFocusResult::ACCEPTED : SetFocusResult::FAILED;
return set_param_int32(Param::FocusMode, focus_param_value) ? SetFocusResult::ACCEPTED : SetFocusResult::FAILED;
}

// set camera lens as a value from 0 to 5
Expand All @@ -265,7 +273,7 @@ bool AP_Mount_Xacti::set_lens(uint8_t lens)
return false;
}

return set_param_int32(XACTI_PARAM_SENSORMODE, lens);
return set_param_int32(Param::SensorMode, lens);
}

// send camera information message to GCS
Expand Down Expand Up @@ -491,13 +499,13 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
{
// display errors
const char* err_prefix_str = "Xacti: failed to";
if (strcmp(name, XACTI_PARAM_SINGLESHOT) == 0) {
if (strcmp(name, get_param_name_str(Param::SingleShot)) == 0) {
if (value < 0) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s take pic", err_prefix_str);
}
return false;
}
if (strcmp(name, XACTI_PARAM_RECORDING) == 0) {
if (strcmp(name, get_param_name_str(Param::Recording)) == 0) {
if (value < 0) {
_recording_video = false;
gcs().send_text(MAV_SEVERITY_ERROR, "%s record", err_prefix_str);
Expand All @@ -507,23 +515,23 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
}
return false;
}
if (strcmp(name, XACTI_PARAM_FOCUSMODE) == 0) {
if (strcmp(name, get_param_name_str(Param::FocusMode)) == 0) {
if (value < 0) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s change focus", err_prefix_str);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "%s %s focus", send_text_prefix, value == 0 ? "manual" : "auto");
}
return false;
}
if (strcmp(name, XACTI_PARAM_SENSORMODE) == 0) {
if (strcmp(name, get_param_name_str(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, "%s %s", send_text_prefix, sensor_mode_str[(uint8_t)value]);
}
return false;
}
if (strcmp(name, XACTI_PARAM_DIGITALZOOM) == 0) {
if (strcmp(name, get_param_name_str(Param::DigitalZoomMagnification)) == 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
Expand All @@ -541,7 +549,7 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
// handle param get/set response
bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, AP_DroneCAN::string &value)
{
if (strcmp(name, XACTI_PARAM_FIRMWAREVERSION) == 0) {
if (strcmp(name, get_param_name_str(Param::FirmwareVersion)) == 0) {
_firmware_version.received = true;
const uint8_t len = MIN(value.len, ARRAY_SIZE(_firmware_version.str)-1);
memcpy(_firmware_version.str, (const char*)value.data, len);
Expand All @@ -560,11 +568,11 @@ bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronec
_firmware_version.mav_ver = UINT32_VALUE(dev_ver_num, patch_ver_num, minor_ver_num, major_ver_num);
}
return false;
} else if (strcmp(name, XACTI_PARAM_DATETIME) == 0) {
} else if (strcmp(name, get_param_name_str(Param::DateTime)) == 0) {
// display when time and date have been set
gcs().send_text(MAV_SEVERITY_INFO, "%s datetime set %s", send_text_prefix, (const char*)value.data);
return false;
} else if (strcmp(name, XACTI_PARAM_STATUS) == 0) {
} else if (strcmp(name, get_param_name_str(Param::Status)) == 0) {
// check for expected length
const char* error_str = "error";
if (value.len != sizeof(_status)) {
Expand All @@ -580,7 +588,11 @@ bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronec
uint32_t changed_bits = last_error_status ^ _status.error_status;
const char* ok_str = "OK";
if (changed_bits & (uint32_t)ErrorStatus::TIME_NOT_SET) {
gcs().send_text(MAV_SEVERITY_INFO, "%s time %sset", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::TIME_NOT_SET ? "not " : " ");
gcs().send_text(MAV_SEVERITY_INFO, "%s time %sset", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::TIME_NOT_SET ? "not " : "");
if (_status.error_status & (uint32_t)ErrorStatus::TIME_NOT_SET) {
// try to set time again
_datetime.set = false;
}
}
if (changed_bits & (uint32_t)ErrorStatus::MEDIA_ERROR) {
gcs().send_text(MAV_SEVERITY_INFO, "%s media %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MEDIA_ERROR ? error_str : ok_str);
Expand All @@ -603,6 +615,7 @@ bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronec

// set motor error for health reporting
_motor_error = _status.error_status & ((uint32_t)ErrorStatus::MOTOR_INIT_ERROR | (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR | (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR);
_camera_error = _status.error_status & ((uint32_t)ErrorStatus::LENS_ERROR | (uint32_t)ErrorStatus::MEDIA_ERROR);
return false;
}

Expand All @@ -619,43 +632,87 @@ void AP_Mount_Xacti::handle_param_save_response(AP_DroneCAN* ap_dronecan, const
}
}

// get parameter name for a particular param enum value
const char* AP_Mount_Xacti::get_param_name_str(Param param) const
{
// check to avoid reading beyond end of array. This should never happen
if ((uint8_t)param > ARRAY_SIZE(_param_names)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return nullptr;
}
return _param_names[(uint8_t)param];
}

// helper function to set integer parameters
bool AP_Mount_Xacti::set_param_int32(const char* param_name, int32_t param_value)
bool AP_Mount_Xacti::set_param_int32(Param param, int32_t param_value)
{
if (_set_param_int32_queue == nullptr) {
return false;
}

// set param request added to queue to be sent. throttling requests improves reliability
return _set_param_int32_queue->push(SetParamQueueItem{param, param_value});
}

// helper function to set string parameters
bool AP_Mount_Xacti::set_param_string(Param param, const AP_DroneCAN::string& 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)) {
// convert param to string
const char* param_name_str = get_param_name_str(param);
if (param_name_str == nullptr) {
return false;
}

if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, param_value, &param_string_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
}
return false;
}

bool AP_Mount_Xacti::set_param_string(const char* param_name, const AP_DroneCAN::string& param_value)
// helper function to get string parameters
bool AP_Mount_Xacti::get_param_string(Param param)
{
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_string_cb)) {
// convert param to string
const char* param_name_str = get_param_name_str(param);
if (param_name_str == nullptr) {
return false;
}

if (_detected_modules[_instance].ap_dronecan->get_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, &param_string_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
}
return false;
}

// helper function to get string parameters
bool AP_Mount_Xacti::get_param_string(const char* param_name)
// process queue of set parameter items
bool AP_Mount_Xacti::process_set_param_int32_queue()
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
if ((_set_param_int32_queue == nullptr) || (_detected_modules[_instance].ap_dronecan == nullptr)) {
return false;
}

if (_detected_modules[_instance].ap_dronecan->get_parameter_on_node(_detected_modules[_instance].node_id, param_name, &param_string_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
SetParamQueueItem param_to_set;
if (_set_param_int32_queue->pop(param_to_set)) {
// convert param to string
const char* param_name_str = get_param_name_str(param_to_set.param);
if (param_name_str == nullptr) {
return false;
}
if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, param_to_set.value, &param_int_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
}
return false;
}
return false;
}
Expand Down Expand Up @@ -745,7 +802,7 @@ bool AP_Mount_Xacti::update_zoom_rate_control(uint32_t now_ms)
}

// send desired zoom to camera
return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_value);
return set_param_int32(Param::DigitalZoomMagnification, zoom_value);
}

// request firmware version. now_ms should be current system time (reduces calls to AP_HAL::millis)
Expand All @@ -762,7 +819,7 @@ bool AP_Mount_Xacti::request_firmware_version(uint32_t now_ms)
return false;
}
_firmware_version.last_request_ms = now_ms;
return get_param_string(XACTI_PARAM_FIRMWAREVERSION);
return get_param_string(Param::FirmwareVersion);
}

// set date and time. now_ms is current system time
Expand Down Expand Up @@ -797,7 +854,7 @@ bool AP_Mount_Xacti::set_datetime(uint32_t now_ms)
return false;
}
datetime_string.len = num_bytes;
_datetime.set = set_param_string(XACTI_PARAM_DATETIME, datetime_string);
_datetime.set = set_param_string(Param::DateTime, datetime_string);
if (!_datetime.set) {
gcs().send_text(MAV_SEVERITY_ERROR, "%s failed to set date/time", send_text_prefix);
}
Expand All @@ -815,7 +872,7 @@ bool AP_Mount_Xacti::request_status(uint32_t now_ms)
}

_status_report.last_request_ms = now_ms;
return get_param_string(XACTI_PARAM_STATUS);
return get_param_string(Param::Status);
}

// check if safe to send message (if messages sent too often camera will not respond)
Expand Down
35 changes: 32 additions & 3 deletions libraries/AP_Mount/AP_Mount_Xacti.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "AP_Mount.h"

class AP_Mount_Xacti : public AP_Mount_Backend
Expand Down Expand Up @@ -118,10 +119,30 @@ class AP_Mount_Xacti : public AP_Mount_Backend
bool handle_param_get_set_response_string(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, AP_DroneCAN::string &value);
void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);

// param enum. If enum is updated also update _param_names definition in cpp
enum class Param : uint8_t {
SingleShot = 0,
Recording,
FocusMode,
SensorMode,
DigitalZoomMagnification,
FirmwareVersion,
Status,
DateTime,
LAST = DateTime, // this should be equal to the final parameter enum
};
static const char* _param_names[]; // array of Xacti parameter strings

// get parameter name for a particular param enum value
const char* get_param_name_str(Param param) const;

// helper function to get and set parameters
bool set_param_int32(const char* param_name, int32_t param_value);
bool set_param_string(const char* param_name, const AP_DroneCAN::string& param_value);
bool get_param_string(const char* param_name);
bool set_param_int32(Param param, int32_t param_value);
bool set_param_string(Param param, const AP_DroneCAN::string& param_value);
bool get_param_string(Param param);

// process queue of set parameter items. returns true if set-parameter message was sent
bool process_set_param_int32_queue();

// send gimbal control message via DroneCAN
// mode is 2:angle control or 3:rate control
Expand Down Expand Up @@ -217,6 +238,7 @@ class AP_Mount_Xacti : public AP_Mount_Backend
uint32_t last_error_status; // last error status reported to user
} _status_report;
bool _motor_error; // true if status reports motor or control error (used for health reporting)
bool _camera_error; // true if status reports camera error

// DroneCAN related variables
static bool _subscribed; // true once subscribed to receive DroneCAN messages
Expand All @@ -229,6 +251,13 @@ class AP_Mount_Xacti : public AP_Mount_Backend
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_getset_param_ms; // system time that a get or set parameter message was sent

// queue of set parameter int32 items. set-parameter requests to camera are throttled to improve reliability
struct SetParamQueueItem {
Param param; // parameter (name)
int32_t value; // parameter value
};
ObjectArray<SetParamQueueItem> *_set_param_int32_queue; // queue of set-parameter items
};

#endif // HAL_MOUNT_XACTI_ENABLED

0 comments on commit 67c160a

Please sign in to comment.