From 288f7b565061641b1598be64078504ef74fbff18 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Sep 2023 17:11:46 +0900 Subject: [PATCH] AP_Mount: Xacti take pic reliability improved --- libraries/AP_Mount/AP_Mount_Xacti.cpp | 143 ++++++++++++++++++-------- libraries/AP_Mount/AP_Mount_Xacti.h | 35 ++++++- 2 files changed, 132 insertions(+), 46 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 1ac71d8302..f3803df8ac 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -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 ¶ms, 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(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, ¶m_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, ¶m_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, ¶m_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, ¶m_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, ¶m_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, ¶m_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) diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 06749f8852..5158d72e43 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -16,6 +16,7 @@ #include #include #include +#include #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 *_set_param_int32_queue; // queue of set-parameter items }; #endif // HAL_MOUNT_XACTI_ENABLED