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 <bapstroman@gmail.com>

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<uint8_t>(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east,
 					    wind.windspeed_north))) * 0.5f);
 			return true;