diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index f898cf7804..96cd830c19 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/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 diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 1867ec5f07..52950dec34 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/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