fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 04:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Fix bug when changing to high latency telemetry

Browse Source 
Due to the old heartbeat of the high latency telemetry when activating it after flying sufficiently long in normal telemetry it is first detected as lost until the first message is sent.
By updating the heartbeat to the current time on switching this issue is avoided.

Also includes a small style fix for the HIGH_LATENCY2 stream
This commit is contained in:
acfloria
2018-03-29 11:05:13 +02:00
committed by Beat Küng
parent 1124a397f2
commit b95d65df53
2 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+8 -1
View File
@@ -4154,7 +4154,7 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout)
_telemetry_high_latency[i] = false;
}
if (telemetry.heartbeat_time > 0) {
if (telemetry.heartbeat_time > 0 && (_telemetry_last_heartbeat[i] < telemetry.heartbeat_time)) {
_telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
}
@@ -4274,6 +4274,13 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
vehicle_cmd.target_system = status.system_id;
vehicle_cmd.target_component = 0;
// set heartbeat to current time for high latency so that the first message can be transmitted
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry_high_latency[i]) {
_telemetry_last_heartbeat[i] = hrt_absolute_time();
}
}
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);