dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps: use all injection data 

When we fall back to another link, we are already doing a uORB copy when
checking the data. Therefore, we should further down use/send that data
instead of overwriting it immediately.

Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
Julian Oes 2023-05-03 12:24:55 +12:00 committed by Beat Küng
parent 2fd3de8f45
commit 1d4b66fcbc
1 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/drivers/gps/gps.cpp
View File

@ -522,6 +522,9 @@ void GPS::handleInjectDataTopic()
return;
}
// We don't want to call copy again further down if we have already done a
// copy in the selection process.
bool already_copied = false;
gps_inject_data_s msg;
// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
@ -531,6 +534,8 @@ void GPS::handleInjectDataTopic()
if (_orb_inject_data_sub.ChangeInstance(i)) {
if (_orb_inject_data_sub.copy(&msg)) {
if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
// Remember that we already did a copy on this instance.
already_copied = true;
_selected_rtcm_instance = i;
break;
}
@ -554,25 +559,26 @@ void GPS::handleInjectDataTopic()
do {
num_injections++;
updated = _orb_inject_data_sub.updated();
updated = already_copied || _orb_inject_data_sub.update(&msg);
if (updated) {
// Prevent injection of data from self
if (msg.device_id != get_device_id()) {
/* Write the message to the gps device. Note that the message could be fragmented.
* But as we don't write anywhere else to the device during operation, we don't
* need to assemble the message first.
*/
injectData(msg.data, msg.len);
if (_orb_inject_data_sub.copy(&msg)) {
// Prevent injection of data from self
if (msg.device_id != get_device_id()) {
/* Write the message to the gps device. Note that the message could be fragmented.
* But as we don't write anywhere else to the device during operation, we don't
* need to assemble the message first.
*/
injectData(msg.data, msg.len);
++_last_rate_rtcm_injection_count;
_last_rtcm_injection_time = hrt_absolute_time();
}
++_last_rate_rtcm_injection_count;
_last_rtcm_injection_time = hrt_absolute_time();
}
}
// Reset the flag, it's only used once.
already_copied = false;
} while (updated && num_injections < max_num_injections);
}