diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index bffc940e29..218b0a4408 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -98,6 +98,7 @@ constexpr uint8_t k_max_command_size = 140; constexpr uint16_t k_timeout_5hz = 500; constexpr uint32_t k_read_buffer_size = 150; constexpr time_t k_gps_epoch_secs = 1234567890ULL; // TODO: This seems wrong +constexpr hrt_abstime k_message_timeout = 6_s; /// A message is considered missing if we havent received it for this period of time // Septentrio receiver commands // - erst: exeResetReceiver @@ -312,6 +313,7 @@ void SeptentrioDriver::run() SEP_INFO("Automatic configuration finished"); _state = State::ReceivingData; + initialize_message_tracker(); } else { _state = State::DetectingBaudRate; @@ -325,7 +327,8 @@ void SeptentrioDriver::run() receive_result = receive(k_timeout_5hz); - if (receive_result == -1) { + if (receive_result == -1 || receiver_configuration_healthy() == false) { + SEP_WARN("Receiver unhealthy, reconfiguring the receiver."); _state = State::DetectingBaudRate; } @@ -1061,7 +1064,7 @@ int SeptentrioDriver::process_message() } case BlockID::DOP: { SEP_TRACE_PARSING("Processing DOP SBF message"); - _current_interval_messages.dop = true; + _message_tracker.dop = hrt_absolute_time(); DOP dop; @@ -1078,7 +1081,7 @@ int SeptentrioDriver::process_message() using Error = PVTGeodetic::Error; SEP_TRACE_PARSING("Processing PVTGeodetic SBF message"); - _current_interval_messages.pvt_geodetic = true; + _message_tracker.pvt_geodetic = hrt_absolute_time(); Header header; PVTGeodetic pvt_geodetic; @@ -1343,7 +1346,7 @@ int SeptentrioDriver::process_message() } case BlockID::VelCovGeodetic: { SEP_TRACE_PARSING("Processing VelCovGeodetic SBF message"); - _current_interval_messages.vel_cov_geodetic = true; + _message_tracker.vel_cov_geodetic = hrt_absolute_time(); VelCovGeodetic vel_cov_geodetic; @@ -1363,7 +1366,7 @@ int SeptentrioDriver::process_message() using Error = AttEuler::Error; SEP_TRACE_PARSING("Processing AttEuler SBF message"); - _current_interval_messages.att_euler = true; + _message_tracker.att_euler = hrt_absolute_time(); AttEuler att_euler; @@ -1388,7 +1391,7 @@ int SeptentrioDriver::process_message() using Error = AttCovEuler::Error; SEP_TRACE_PARSING("Processing AttCovEuler SBF message"); - _current_interval_messages.att_cov_euler = true; + _message_tracker.att_cov_euler = hrt_absolute_time(); AttCovEuler att_cov_euler; @@ -1780,11 +1783,9 @@ void SeptentrioDriver::start_update_monitoring_interval() _last_interval_rtcm_injections = _current_interval_rtcm_injections; _last_interval_bytes_written = _current_interval_bytes_written; _last_interval_bytes_read = _current_interval_bytes_read; - _last_interval_messages = _current_interval_messages; _current_interval_rtcm_injections = 0; _current_interval_bytes_written = 0; _current_interval_bytes_read = 0; - _current_interval_messages = MessageTracker {}; _current_interval_start_time = hrt_absolute_time(); } @@ -1815,11 +1816,19 @@ uint32_t SeptentrioDriver::input_data_rate() const bool SeptentrioDriver::receiver_configuration_healthy() const { - return _last_interval_messages.dop && - _last_interval_messages.pvt_geodetic && - _last_interval_messages.vel_cov_geodetic && - _last_interval_messages.att_euler && - _last_interval_messages.att_cov_euler; + return hrt_elapsed_time(&_message_tracker.dop) <= k_message_timeout && + hrt_elapsed_time(&_message_tracker.pvt_geodetic) <= k_message_timeout && + hrt_elapsed_time(&_message_tracker.vel_cov_geodetic) <= k_message_timeout && + hrt_elapsed_time(&_message_tracker.att_euler) <= k_message_timeout && + hrt_elapsed_time(&_message_tracker.att_cov_euler) <= k_message_timeout; +} + +void SeptentrioDriver::initialize_message_tracker(){ + _message_tracker.dop = hrt_absolute_time(); + _message_tracker.pvt_geodetic = hrt_absolute_time(); + _message_tracker.vel_cov_geodetic = hrt_absolute_time(); + _message_tracker.att_euler = hrt_absolute_time(); + _message_tracker.att_cov_euler = hrt_absolute_time(); } float SeptentrioDriver::us_to_s(uint64_t us) diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index 78761432a9..c8fcd36040 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -205,11 +205,11 @@ enum class SBFOutputFrequency : int32_t { * Tracker for messages received by the driver. */ struct MessageTracker { - bool dop {false}; - bool pvt_geodetic {false}; - bool vel_cov_geodetic {false}; - bool att_euler {false}; - bool att_cov_euler {false}; + hrt_abstime dop {0}; + hrt_abstime pvt_geodetic {0}; + hrt_abstime vel_cov_geodetic {0}; + hrt_abstime att_euler {0}; + hrt_abstime att_cov_euler {0}; }; /** @@ -673,6 +673,14 @@ private: */ void reset_gps_state_message(); + /** + * @brief Initialize the message tracker by setting all tracked message timestamps to the current time. + * + * This prevents false health-check failures immediately after (re-)configuration by treating all messages + * as freshly received. + */ + void initialize_message_tracker(); + /** * @brief Get the parameter with the given name into `value`. * @@ -767,8 +775,7 @@ private: uint32_t _current_interval_bytes_written {0}; ///< Nr of bytes written to the receiver in the current measurement interval uint32_t _last_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the last measurement interval uint32_t _current_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the current measurement interval - MessageTracker _last_interval_messages {}; ///< Messages encountered in the last measurement interval - MessageTracker _current_interval_messages {}; ///< Messages encountered in the current measurement interval + MessageTracker _message_tracker {}; ///< Tracks the last reception time of important messages for monitoring whether the receiver is configured correctly and healthy }; } // namespace septentrio