From b95d65df53ffbd7d96abc8b5cfc94f597e214e46 Mon Sep 17 00:00:00 2001 From: acfloria Date: Thu, 29 Mar 2018 11:05:13 +0200 Subject: [PATCH] Commander: Fix bug when changing to high latency telemetry 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 --- src/modules/commander/commander.cpp | 9 ++++++++- src/modules/mavlink/mavlink_messages.cpp | 14 +++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2075338ede..dc461adb9d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 846f32f9cf..ee54056eb7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -4448,7 +4448,7 @@ protected: void update_airspeed(float update_rate) { - struct airspeed_s airspeed = {}; + airspeed_s airspeed = {}; if (_airspeed_sub->update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, update_rate); @@ -4458,7 +4458,7 @@ protected: void update_tecs_status(float update_rate) { - struct tecs_status_s tecs_status = {}; + tecs_status_s tecs_status = {}; if (_tecs_status_sub->update(&tecs_status)) { _airspeed_sp.add_value(tecs_status.airspeedSp, update_rate); @@ -4467,7 +4467,7 @@ protected: void update_battery_status(float update_rate) { - struct battery_status_s battery = {}; + battery_status_s battery = {}; if (_battery_sub->update(&battery)) { _battery.add_value(battery.remaining, update_rate); @@ -4476,7 +4476,7 @@ protected: void update_global_position(float update_rate) { - struct vehicle_global_position_s global_pos = {}; + vehicle_global_position_s global_pos = {}; if (_global_pos_sub->update(&global_pos)) { _climb_rate.add_value(fabsf(global_pos.vel_d), update_rate); @@ -4486,7 +4486,7 @@ protected: void update_gps(float update_rate) { - struct vehicle_gps_position_s gps = {}; + vehicle_gps_position_s gps = {}; if (_gps_sub->update(&gps)) { _eph.add_value(gps.eph, update_rate); @@ -4496,7 +4496,7 @@ protected: void update_vehicle_status(float update_rate) { - struct vehicle_status_s status = {}; + vehicle_status_s status = {}; if (_status_sub->update(&status)) { if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { @@ -4521,7 +4521,7 @@ protected: void update_wind_estimate(float update_rate) { - struct wind_estimate_s wind = {}; + wind_estimate_s wind = {}; if (_wind_sub->update(&wind)) { _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),