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

AP_Mount: Xacti take pic reliability improved

This commit is contained in:
Randy Mackay 2023-09-20 17:11:46 +09:00 committed by Andrew Tridgell
parent 4ba6375d38
commit 288f7b5650
2 changed files with 132 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -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)
@ -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) :
@ -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;
}
@ -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
@ -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
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
@ -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
@ -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
@ -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
@ -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);
@ -507,7 +515,7 @@ 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 {
@ -515,7 +523,7 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
}
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)) {
@ -523,7 +531,7 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
}
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
@ -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);
@ -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)) {
@ -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);
@ -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;
}
@ -619,27 +632,42 @@ 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)
// get parameter name for a particular param enum value
const char* AP_Mount_Xacti::get_param_name_str(Param param) const
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return false;
// 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;
}
if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name, param_value, &param_int_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
}
return false;
return _param_names[(uint8_t)param];
}
bool AP_Mount_Xacti::set_param_string(const char* param_name, const AP_DroneCAN::string& param_value)
// helper function to set integer parameters
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_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->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;
}
@ -647,19 +675,48 @@ bool AP_Mount_Xacti::set_param_string(const char* param_name, const AP_DroneCAN:
}
// helper function to get string parameters
bool AP_Mount_Xacti::get_param_string(const char* param_name)
bool AP_Mount_Xacti::get_param_string(Param param)
{
if (_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)) {
// 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;
}
// process queue of set parameter items
bool AP_Mount_Xacti::process_set_param_int32_queue()
{
if ((_set_param_int32_queue == nullptr) || (_detected_modules[_instance].ap_dronecan == nullptr)) {
return false;
}
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;
}
// 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
@ -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)
@ -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
@ -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);
}
@ -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)

35
libraries/AP_Mount/AP_Mount_Xacti.h
View File

@ -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
@ -118,10 +119,30 @@ private:
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
@ -217,6 +238,7 @@ private:
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
@ -229,6 +251,13 @@ private:
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