diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 50405f765d..98d0c3d091 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -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 ¶ms, 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, ¶m_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, ¶m_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, ¶m_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, ¶m_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 diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 50aee42d8a..d8c140f1db 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -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