AP_Mount: gremsy implements healthy

libraries/AP_Mount/AP_Mount_Gremsy.cpp

@@ -88,6 +88,34 @@ void AP_Mount_Gremsy::update()
     }
 }
 
+// return true if healthy
+bool AP_Mount_Gremsy::healthy() const
+{
+    // unhealthy until gimbal has been found and replied with device info
+    if (!_found_gimbal || !_got_device_info) {
+        return false;
+    }
+
+    // unhealthy if attitude information NOT received within the last second
+    if (AP_HAL::millis() - _last_attitude_status_ms > 1000) {
+        return false;
+    }
+
+    // check failure flags
+    uint32_t critical_failure_flags = GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR |
+                                      GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR |
+                                      GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR |
+                                      GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR |
+                                      GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR;
+
+    if ((_gimbal_device_attitude_status.failure_flags & critical_failure_flags) > 0) {
+        return false;
+    }
+
+    // if we get this far return mount is healthy
+    return true;
+}
+
 // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
 void AP_Mount_Gremsy::send_mount_status(mavlink_channel_t chan)
 {
@@ -208,6 +236,7 @@ void AP_Mount_Gremsy::handle_gimbal_device_attitude_status(const mavlink_message
 
     // take copy of message so it can be forwarded onto GCS later
     mavlink_msg_gimbal_device_attitude_status_decode(&msg, &_gimbal_device_attitude_status);
+    _last_attitude_status_ms = AP_HAL::millis();
 }
 
 // request GIMBAL_DEVICE_INFORMATION message

libraries/AP_Mount/AP_Mount_Gremsy.h

@@ -30,6 +30,9 @@ public:
     // update mount position
     void update() override;
 
+    // return true if healthy
+    bool healthy() const override;
+
     // has_pan_control
     bool has_pan_control() const override { return true; }
 
@@ -77,6 +80,7 @@ private:
     uint8_t _sysid;                 // sysid of gimbal
     uint8_t _compid;                // component id of gimbal
     mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status;  // copy of most recently received gimbal status
+    uint32_t _last_attitude_status_ms;  // system time last attitude status was received (used for health reporting)
     uint32_t _sent_gimbal_device_attitude_status_ms;    // time_boot_ms field of gimbal_device_status message last forwarded to the GCS (used to prevent sending duplicates)
 };
 #endif // HAL_MOUNT_GREMSY_ENABLED