From 760bcdec2fff40590bfdc6f65848c8ca44446a41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Tue, 14 Feb 2023 16:58:09 +0100 Subject: [PATCH] high_latency_stream: fixed bug where fields were not updating - topic update was checked twice in the same loop and thus the second time the topic would never indicate to have updated Co-authored-by: RomanBapst --- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 53 ++++++++++--------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index c349340ac3..a7d522e063 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -125,18 +125,20 @@ private: updated |= _temperature.valid(); updated |= _throttle.valid(); updated |= _windspeed.valid(); - updated |= write_airspeed(&msg); - updated |= write_attitude_sp(&msg); - updated |= write_battery_status(&msg); - updated |= write_estimator_status(&msg); - updated |= write_fw_ctrl_status(&msg); - updated |= write_geofence_result(&msg); - updated |= write_global_position(&msg); - updated |= write_mission_result(&msg); - updated |= write_tecs_status(&msg); - updated |= write_vehicle_status(&msg); + updated |= write_attitude_setpoint_if_updated(&msg); + updated |= write_estimator_status_if_updated(&msg); + updated |= write_fw_ctrl_status_if_updated(&msg); + updated |= write_geofence_result_if_updated(&msg); + updated |= write_global_position_if_updated(&msg); + updated |= write_mission_result_if_updated(&msg); updated |= write_failsafe_flags(&msg); - updated |= write_wind(&msg); + + // these topics are already updated in update_data() and thus we just copy them here + write_airspeed(&msg); + write_battery_status(&msg); + write_tecs_status(&msg); + write_vehicle_status(&msg); + write_wind(&msg); if (updated) { msg.timestamp = t / 1000; @@ -244,7 +246,7 @@ private: { airspeed_s airspeed; - if (_airspeed_sub.update(&airspeed)) { + if (_airspeed_sub.copy(&airspeed)) { if (airspeed.confidence < 0.95f) { // the same threshold as for the commander msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; } @@ -255,7 +257,7 @@ private: return false; } - bool write_attitude_sp(mavlink_high_latency2_t *msg) + bool write_attitude_setpoint_if_updated(mavlink_high_latency2_t *msg) { vehicle_attitude_setpoint_s attitude_sp; @@ -274,7 +276,7 @@ private: for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { battery_status_s battery; - if (_batteries[i].subscription.update(&battery)) { + if (_batteries[i].subscription.copy(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -287,11 +289,12 @@ private: return updated; } - bool write_estimator_status(mavlink_high_latency2_t *msg) + bool write_estimator_status_if_updated(mavlink_high_latency2_t *msg) { // use primary estimator_status - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.update(&estimator_selector_status)) { if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { @@ -320,7 +323,7 @@ private: return false; } - bool write_fw_ctrl_status(mavlink_high_latency2_t *msg) + bool write_fw_ctrl_status_if_updated(mavlink_high_latency2_t *msg) { position_controller_status_s pos_ctrl_status; @@ -334,7 +337,7 @@ private: return false; } - bool write_geofence_result(mavlink_high_latency2_t *msg) + bool write_geofence_result_if_updated(mavlink_high_latency2_t *msg) { geofence_result_s geofence; @@ -350,12 +353,12 @@ private: return false; } - bool write_global_position(mavlink_high_latency2_t *msg) + bool write_global_position_if_updated(mavlink_high_latency2_t *msg) { vehicle_global_position_s global_pos; vehicle_local_position_s local_pos; - if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) { + if (_global_pos_sub.update(&global_pos) && _local_pos_sub.copy(&local_pos)) { msg->latitude = global_pos.lat * 1e7; msg->longitude = global_pos.lon * 1e7; @@ -378,7 +381,7 @@ private: return false; } - bool write_mission_result(mavlink_high_latency2_t *msg) + bool write_mission_result_if_updated(mavlink_high_latency2_t *msg) { mission_result_s mission_result; @@ -394,7 +397,7 @@ private: { tecs_status_s tecs_status; - if (_tecs_status_sub.update(&tecs_status)) { + if (_tecs_status_sub.copy(&tecs_status)) { int16_t target_altitude; convert_limit_safe(tecs_status.altitude_sp, target_altitude); msg->target_altitude = target_altitude; @@ -409,7 +412,7 @@ private: { vehicle_status_s status; - if (_status_sub.update(&status)) { + if (_status_sub.copy(&status)) { health_report_s health_report; if (_health_report_sub.copy(&health_report)) { @@ -476,7 +479,7 @@ private: { wind_s wind; - if (_wind_sub.update(&wind)) { + if (_wind_sub.copy(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f); return true;