AP_Mount: Xacti handles firmware ver and status

libraries/AP_Mount/AP_Mount_Xacti.cpp

@@ -16,9 +16,12 @@ extern const AP_HAL::HAL& hal;
 #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_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 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)
@@ -36,6 +39,7 @@ AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params &
     register_backend();
 
     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);
 }
 
@@ -58,6 +62,16 @@ void AP_Mount_Xacti::update()
         return;
     }
 
+    // get firmware version
+    if (request_firmware_version()) {
+        return;
+    }
+
+    // request status
+    if (request_status()) {
+        return;
+    }
+
     // periodically send copter attitude and GPS status
     if (send_copter_att_status()) {
         // if message sent avoid sending other messages
@@ -148,8 +162,8 @@ void AP_Mount_Xacti::update()
 // return true if healthy
 bool AP_Mount_Xacti::healthy() const
 {
-    // unhealthy until gimbal has been found and replied with firmware version info
+    // unhealthy until gimbal has been found and replied with firmware version and no motor errors
-    if (!_initialised) {
+    if (!_initialised || !_firmware_version.received || _motor_error) {
         return false;
     }
 
@@ -172,7 +186,6 @@ bool AP_Mount_Xacti::take_picture()
 
     // set SingleShot parameter
     return set_param_int32(XACTI_PARAM_SINGLESHOT, 0);
-
 }
 
 // start or stop video recording.  returns true on success
@@ -271,7 +284,7 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const
         AP_HAL::millis(),       // time_boot_ms
         vendor_name,            // vendor_name uint8_t[32]
         model_name,             // model_name uint8_t[32]
-        0,                      // firmware version uint32_t
+        _firmware_version.received ? _firmware_version.mav_ver : 0, // firmware version uint32_t
         NaN,                    // focal_length float (mm)
         NaN,                    // sensor_size_h float (mm)
         NaN,                    // sensor_size_v float (mm)
@@ -517,6 +530,78 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
     return false;
 }
 
+// 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) {
+        _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);
+        gcs().send_text(MAV_SEVERITY_INFO, "Mount: Xacti fw:%s", _firmware_version.str);
+
+        // firmware str from gimbal is of the format YYMMDD[b]xx.  Convert to uint32 for reporting to GCS
+        if (len >= 9) {
+            const char major_str[3] = {_firmware_version.str[0], _firmware_version.str[1], 0};
+            const char minor_str[3] = {_firmware_version.str[2], _firmware_version.str[3], 0};
+            const char patch_str[3] = {_firmware_version.str[4], _firmware_version.str[5], 0};
+            const char dev_str[3] = {_firmware_version.str[7], _firmware_version.str[8], 0};
+            const uint8_t major_ver_num = atoi(major_str) & 0xFF;
+            const uint8_t minor_ver_num = atoi(minor_str) & 0xFF;
+            const uint8_t patch_ver_num = atoi(patch_str) & 0xFF;
+            const uint8_t dev_ver_num = atoi(dev_str) & 0xFF;
+            _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_STATUS) == 0) {
+        // check for expected length
+        const char* error_str = "error";
+        if (value.len != sizeof(_status)) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "%s status len %s", send_text_prefix, error_str);
+            return false;
+        }
+
+        // backup error status and copy to structure
+        const uint32_t last_error_status = _status.error_status;
+        memcpy(&_status, value.data, value.len);
+
+        // report change in status
+        uint32_t changed_bits = last_error_status ^ _status.error_status;
+        const char* ok_str = "OK";
+        if (changed_bits & (uint32_t)ErrorStatus::CANNOT_TAKE_PIC) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s %s take pic", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::CANNOT_TAKE_PIC ? "cannot" : "can");
+        }
+        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 " : " ");
+        }
+        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);
+        }
+        if (changed_bits & (uint32_t)ErrorStatus::LENS_ERROR) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s lens %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::LENS_ERROR ? error_str : ok_str);
+        }
+        if (changed_bits & (uint32_t)ErrorStatus::MOTOR_INIT_ERROR) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s motor %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MOTOR_INIT_ERROR ? "init error" : ok_str);
+        }
+        if (changed_bits & (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s motor op %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR ? error_str : ok_str);
+        }
+        if (changed_bits & (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s control %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR ? error_str : ok_str);
+        }
+        if (changed_bits & (uint32_t)ErrorStatus::TEMP_WARNING) {
+            gcs().send_text(MAV_SEVERITY_INFO, "%s temp %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::TEMP_WARNING ? "warning" : ok_str);
+        }
+
+        // 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);
+        return false;
+    }
+
+    // unhandled parameter get or set
+    gcs().send_text(MAV_SEVERITY_INFO, "%s get/set string %s res:%s", send_text_prefix, name, (const char*)value.data);
+    return false;
+}
+
 void AP_Mount_Xacti::handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success)
 {
     // display failure to save parameter
@@ -533,7 +618,21 @@ bool AP_Mount_Xacti::set_param_int32(const char* param_name, int32_t param_value
     }
 
     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();
+        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)
+{
+    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)) {
+        last_send_getset_param_ms = AP_HAL::millis();
         return true;
     }
     return false;
@@ -629,6 +728,38 @@ bool AP_Mount_Xacti::update_zoom_rate_control()
     return set_param_int32(XACTI_PARAM_DIGITALZOOM, zoom_value);
 }
 
+// request firmware version
+// returns true if sent so that we avoid immediately trying to also send other messages
+bool AP_Mount_Xacti::request_firmware_version()
+{
+    // return immediately if already have version or no dronecan
+    if (_firmware_version.received) {
+        return false;
+    }
+
+    // send request once per second until received
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _firmware_version.last_request_ms < 1000) {
+        return false;
+    }
+    _firmware_version.last_request_ms = now_ms;
+    return get_param_string(XACTI_PARAM_FIRMWAREVERSION);
+}
+
+// request status
+// returns true if sent so that we avoid immediately trying to also send other messages
+bool AP_Mount_Xacti::request_status()
+{
+    // return immediately if 3 seconds has not passed
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _status_report.last_request_ms < XACTI_STATUS_REQ_INTERVAL_MS) {
+        return false;
+    }
+
+    _status_report.last_request_ms = now_ms;
+    return get_param_string(XACTI_PARAM_STATUS);
+}
+
 // check if safe to send message (if messages sent too often camera will not respond)
 bool AP_Mount_Xacti::is_safe_to_send() const
 {
@@ -645,7 +776,7 @@ bool AP_Mount_Xacti::is_safe_to_send() const
     }
 
     // check time since last set param message sent
-    if (now_ms - last_send_set_param_ms < XACTI_MSG_SEND_MIN_MS) {
+    if (now_ms - last_send_getset_param_ms < XACTI_MSG_SEND_MIN_MS) {
         return false;
     }
 

libraries/AP_Mount/AP_Mount_Xacti.h

@@ -112,12 +112,15 @@ private:
 
     // DroneCAN parameter handling methods
     FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
+    FUNCTOR_DECLARE(param_string_cb, bool, AP_DroneCAN*, const uint8_t, const char*, AP_DroneCAN::string &);
     FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool);
     bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
+    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);
 
-    // helper function to set integer parameters
+    // helper function to get and set parameters
     bool set_param_int32(const char* param_name, int32_t param_value);
+    bool get_param_string(const char* param_name);
 
     // send gimbal control message via DroneCAN
     // mode is 2:angle control or 3:rate control
@@ -133,6 +136,14 @@ private:
     // returns true if sent so that we avoid immediately trying to also send other messages
     bool update_zoom_rate_control();
 
+    // request firmware version
+    // returns true if sent so that we avoid immediately trying to also send other messages
+    bool request_firmware_version();
+
+    // request status
+    // returns true if sent so that we avoid immediately trying to also send other messages
+    bool request_status();
+
     // check if safe to send message (if messages sent too often camera will not respond)
     bool is_safe_to_send() const;
 
@@ -150,6 +161,52 @@ private:
         uint32_t last_update_ms;                    // system time that zoom rate control last updated zoom
     } _zoom_rate_control;
 
+    // firmware version received from gimbal
+    struct {
+        uint32_t last_request_ms;                   // system time of last request for firmware version
+        bool received;                              // true once firmware version has been received
+        char str[12] {};                            // firmware version string (11 bytes + 1 null byte)
+        uint32_t mav_ver;                           // version formatted for reporting to GCS via CAMERA_INFORMATION message
+    } _firmware_version;
+
+    // gimbal status handling
+    enum class ErrorStatus : uint32_t {
+        TAKING_PICTURE = 0x04,                      // currently taking a picture
+        RECORDING_VIDEO = 0x08,                     // currently recording video
+        CANNOT_TAKE_PIC = 0x20,
+        TIME_NOT_SET = 0x10000,
+        MEDIA_ERROR = 0x20000,
+        LENS_ERROR = 0x40000,
+        MOTOR_INIT_ERROR = 0x100000,
+        MOTOR_OPERATION_ERROR = 0x200000,
+        GIMBAL_CONTROL_ERROR = 0x400000,
+        TEMP_WARNING = 0x1000000
+    };
+    struct {
+        uint32_t error_status;                      // see ErrorStatus enum
+        uint32_t video_remain_time;                 // max seconds of video that may be recorded before SD card is full
+        uint32_t photo_remain_count;                // max number of pics before SD card is full
+        uint32_t sd_card_size_mb;                   // SD card size in MB
+        uint32_t sd_card_free_mb;                   // SD card remaining size in MB
+        uint16_t body;                              // body type
+        uint16_t cmos;                              // cmos type
+        uint16_t gimbal_pitch;                      // gimbal pitch angle
+        uint16_t gimbal_roll;                       // gimbal roll angle
+        uint16_t gimbal_yaw;                        // gimbal yaw angle
+        uint16_t reserved1;
+        uint8_t date_time[7];                       // camera's date and time
+        uint8_t reserved2;
+        uint32_t exposure_time_us;                  // camera's exposure time in us
+        uint16_t apeture;                           // cameras' aperture * 100
+        uint16_t iso_sensitivity;                   // camera's iso sensitivity
+    } _status;                                      // latest status received
+    static_assert(sizeof(_status) == 48);           // status should be 48 bytes
+    struct {
+        uint32_t last_request_ms;                   // system time that status was last requested
+        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)
+
     // DroneCAN related variables
     static bool _subscribed;                        // true once subscribed to receive DroneCAN messages
     static struct DetectedModules {
@@ -160,7 +217,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
+    uint32_t last_send_getset_param_ms;             // system time that a get or set parameter message was sent
 };
 
 #endif // HAL_MOUNT_XACTI_ENABLED