UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix: track message timeout and reconfigure module when unhealthy

Browse Source
This commit is contained in:
Phil-Engljaehringer
2026-05-28 14:58:36 +02:00
committed by Alexander Lerach
parent 8ddbedf868
commit 1952562310
2 changed files with 36 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
src/drivers/gnss/septentrio/septentrio.cpp
View File

@@ -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)

21
src/drivers/gnss/septentrio/septentrio.h
View File

@@ -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