mavlink messages delete unnecessary timestamps and cleanup

src/modules/mavlink/mavlink_messages.cpp

@@ -523,6 +523,10 @@ private:
 	MavlinkOrbSubscription *_cpuload_sub;
 	MavlinkOrbSubscription *_battery_status_sub;
 
+	uint64_t _status_timestamp{0};
+	uint64_t _cpuload_timestamp{0};
+	uint64_t _battery_status_timestamp{0};
+
 	/* do not allow top copying this class */
 	MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
 	MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &);
@@ -536,13 +540,13 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status = {};
+		vehicle_status_s status = {};
-		struct cpuload_s cpuload = {};
+		cpuload_s cpuload = {};
-		struct battery_status_s battery_status = {};
+		battery_status_s battery_status = {};
 
-		const bool updated_status = _status_sub->update(&status);
+		const bool updated_status = _status_sub->update(&_status_timestamp, &status);
-		const bool updated_cpuload = _cpuload_sub->update(&cpuload);
+		const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload);
-		const bool updated_battery = _battery_status_sub->update(&battery_status);
+		const bool updated_battery = _battery_status_sub->update(&_battery_status_timestamp, &battery_status);
 
 		if (updated_status || updated_battery || updated_cpuload) {
 			mavlink_sys_status_t msg = {};
@@ -638,10 +642,7 @@ private:
 	uint64_t _sensor_time;
 
 	MavlinkOrbSubscription *_bias_sub;
-	uint64_t _bias_time;
-
 	MavlinkOrbSubscription *_differential_pressure_sub;
-	uint64_t _differential_pressure_time;
 
 	uint64_t _accel_timestamp;
 	uint64_t _gyro_timestamp;
@@ -657,9 +658,7 @@ protected:
 		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
 		_sensor_time(0),
 		_bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))),
-		_bias_time(0),
 		_differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))),
-		_differential_pressure_time(0),
 		_accel_timestamp(0),
 		_gyro_timestamp(0),
 		_mag_timestamp(0),
@@ -668,9 +667,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct sensor_combined_s sensor = {};
+		sensor_combined_s sensor;
-		struct sensor_bias_s bias = {};
-		struct differential_pressure_s differential_pressure = {};
 
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
@@ -699,8 +696,10 @@ protected:
 				_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 			}
 
-			_bias_sub->update(&_bias_time, &bias);
+			sensor_bias_s bias = {};
-			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
+			_bias_sub->update(&bias);
+			differential_pressure_s differential_pressure = {};
+			_differential_pressure_sub->update(&differential_pressure);
 
 			mavlink_highres_imu_t msg = {};
 
@@ -769,7 +768,6 @@ private:
 	MavlinkOrbSubscription *_raw_gyro_sub;
 	uint64_t _raw_accel_time;
 	uint64_t _raw_gyro_time;
-	struct sensor_gyro_s _sensor_gyro;
 
 	// do not allow top copy this class
 	MavlinkStreamScaledIMU(MavlinkStreamScaledIMU &);
@@ -780,19 +778,19 @@ protected:
 		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel))),
 		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro))),
 		_raw_accel_time(0),
-		_raw_gyro_time(0),
+		_raw_gyro_time(0)
-		_sensor_gyro{}
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		struct sensor_accel_s sensor_accel = {};
+		sensor_accel_s sensor_accel = {};
+		sensor_accel_s sensor_gyro = {};
 
-		bool raw_accel_updated = _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
+		bool updated = false;
-		_raw_gyro_sub->update(&_raw_gyro_time, &_sensor_gyro);
+		updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
+		updated |= _raw_gyro_sub->update(&_raw_gyro_time, &sensor_gyro);
 
-		// send if raw_accel has been updated and use the newest gyro values we have
+		if (updated) {
-		if (raw_accel_updated) {
 
 			mavlink_scaled_imu_t msg = {};
 
@@ -800,9 +798,9 @@ protected:
 			msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
 			msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
 			msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
-			msg.xgyro = _sensor_gyro.x_raw; // [milli rad/s]
+			msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
-			msg.ygyro = _sensor_gyro.y_raw; // [milli rad/s]
+			msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
-			msg.zgyro = _sensor_gyro.z_raw; // [milli rad/s]
+			msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
 			msg.xmag = 0;
 			msg.ymag = 0;
 			msg.zmag = 0;
@@ -867,7 +865,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_attitude_s att;
+		vehicle_attitude_s att;
 
 		if (_att_sub->update(&_att_time, &att)) {
 			mavlink_attitude_t msg = {};
@@ -939,7 +937,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_attitude_s att;
+		vehicle_attitude_s att;
 
 		if (_att_sub->update(&_att_time, &att)) {
 			mavlink_attitude_quaternion_t msg = {};
@@ -999,12 +997,22 @@ public:
 
 private:
 	MavlinkOrbSubscription *_att_sub;
+	uint64_t _att_time;
+
 	MavlinkOrbSubscription *_pos_sub;
+	uint64_t _pos_time;
+
 	MavlinkOrbSubscription *_armed_sub;
+	uint64_t _armed_time;
+
 	MavlinkOrbSubscription *_act0_sub;
 	MavlinkOrbSubscription *_act1_sub;
+
 	MavlinkOrbSubscription *_airspeed_sub;
+	uint64_t _airspeed_time;
+
 	MavlinkOrbSubscription *_sensor_sub;
+	uint64_t _sensor_time;
 
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
@@ -1013,11 +1021,15 @@ private:
 protected:
 	explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+		_att_time(0),
 		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+		_pos_time(0),
 		_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
+		_armed_time(0),
 		_act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
+		_airspeed_time(0),
 		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
 	{}
 
@@ -1027,13 +1039,13 @@ protected:
 		vehicle_local_position_s pos = {};
 		actuator_armed_s armed = {};
 		airspeed_s airspeed = {};
-		sensor_combined_s sensor = {};
 
-		bool updated = _att_sub->update(&att);
+
-		updated |= _pos_sub->update(&pos);
+		bool updated = false;
-		updated |= _armed_sub->update(&armed);
+		updated |= _att_sub->update(&_att_time, &att);
-		updated |= _airspeed_sub->update(&airspeed);
+		updated |= _pos_sub->update(&_pos_time, &pos);
-		updated |= _sensor_sub->update(&sensor);
+		updated |= _armed_sub->update(&_armed_time, &armed);
+		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg = {};
@@ -1066,6 +1078,9 @@ protected:
 				msg.alt = -pos.z + pos.ref_alt;
 
 			} else {
+				sensor_combined_s sensor = {};
+				_sensor_sub->update(&sensor);
+
 				/* fall back to baro altitude */
 				if (sensor.timestamp > 0) {
 					msg.alt = sensor.baro_alt_meter;
@@ -1135,7 +1150,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_gps_position_s gps;
+		vehicle_gps_position_s gps;
 
 		if (_gps_sub->update(&_gps_time, &gps)) {
 			mavlink_gps_raw_int_t msg = {};
@@ -1152,9 +1167,9 @@ protected:
 			msg.v_acc = gps.epv * 1e3f;
 			msg.vel_acc = gps.s_variance_m_s * 1e3f;
 			msg.hdg_acc = gps.c_variance_rad * 1e5f / M_DEG_TO_RAD_F;
-			msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
+			msg.vel = cm_uint16_from_m_float(gps.vel_m_s);
-			    msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+			msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f;
-				msg.satellites_visible = gps.satellites_used;
+			msg.satellites_visible = gps.satellites_used;
 
 			mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
 
@@ -1679,7 +1694,11 @@ public:
 
 private:
 	MavlinkOrbSubscription *_gpos_sub;
+	uint64_t _gpos_time;
+
 	MavlinkOrbSubscription *_lpos_sub;
+	uint64_t _lpos_time;
+
 	MavlinkOrbSubscription *_home_sub;
 	MavlinkOrbSubscription *_sensor_sub;
 
@@ -1690,7 +1709,9 @@ private:
 protected:
 	explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_gpos_time(0),
 		_lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+		_lpos_time(0),
 		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
 		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
 	{}
@@ -1699,16 +1720,11 @@ protected:
 	{
 		vehicle_global_position_s gpos = {};
 		vehicle_local_position_s lpos = {};
-		home_position_s home = {};
-		sensor_combined_s sensor = {};
 
-		bool updated = false;
+		bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos);
-		updated |= _gpos_sub->update(&gpos);
+		bool lpos_updated = _lpos_sub->update(&_lpos_time, &lpos);
-		updated |= _lpos_sub->update(&lpos);
-		updated |= _home_sub->update(&home);
-		updated |= _sensor_sub->update(&sensor);
 
-		if (updated) {
+		if (gpos_updated && lpos_updated) {
 			mavlink_global_position_int_t msg = {};
 
 			if (lpos.z_valid && lpos.z_global) {
@@ -1716,12 +1732,18 @@ protected:
 
 			} else {
 				// fall back to baro altitude
+				sensor_combined_s sensor = {};
+				_sensor_sub->update(&sensor);
+
 				if (sensor.timestamp > 0) {
 					msg.alt = sensor.baro_alt_meter * 1000.0f;
 				}
 			}
 
-			if (home.valid_alt) {
+			home_position_s home = {};
+			_home_sub->update(&home);
+
+			if ((home.timestamp > 0) && home.valid_alt) {
 				if (lpos.z_valid) {
 					msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
 
@@ -1746,9 +1768,11 @@ protected:
 			msg.hdg = _wrap_2pi(lpos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
 			mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
 
-		return updated;
+		return false;
 	}
 };
 
@@ -1806,8 +1830,8 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_local_position_s vpos = {};
+		vehicle_local_position_s vpos = {};
-		struct vehicle_attitude_s vatt = {};
+		vehicle_attitude_s vatt = {};
 
 		bool att_updated = _att_sub->update(&_att_time, &vatt);
 		bool pos_updated = _pos_sub->update(&_pos_time, &vpos);
@@ -1880,7 +1904,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_local_position_s pos;
+		vehicle_local_position_s pos;
 
 		if (_pos_sub->update(&_pos_time, &pos)) {
 			mavlink_local_position_ned_t msg = {};
@@ -2034,32 +2058,29 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct estimator_status_s est;
+		estimator_status_s est;
 
 		if (_est_sub->update(&_est_time, &est)) {
-
+			// ESTIMATOR_STATUS
-			mavlink_estimator_status_t est_msg = {
+			mavlink_estimator_status_t est_msg = {};
-				.time_usec = est.timestamp,
+			est_msg.time_usec = est.timestamp;
-				.vel_ratio = est.vel_test_ratio,
+			est_msg.vel_ratio = est.vel_test_ratio;
-				.pos_horiz_ratio = est.pos_test_ratio,
+			est_msg.pos_horiz_ratio = est.pos_test_ratio;
-				.pos_vert_ratio = est.hgt_test_ratio,
+			est_msg.pos_vert_ratio = est.hgt_test_ratio;
-				.mag_ratio = est.mag_test_ratio,
+			est_msg.mag_ratio = est.mag_test_ratio;
-				.hagl_ratio = est.hagl_test_ratio,
+			est_msg.hagl_ratio = est.hagl_test_ratio;
-				.tas_ratio = est.tas_test_ratio,
+			est_msg.tas_ratio = est.tas_test_ratio;
-				.pos_horiz_accuracy = est.pos_horiz_accuracy,
+			est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy;
-				.pos_vert_accuracy = est.pos_vert_accuracy,
+			est_msg.pos_vert_accuracy = est.pos_vert_accuracy;
-				.flags = est.solution_status_flags
+			est_msg.flags = est.solution_status_flags;
-			};
-
 			mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg);
 
-			mavlink_vibration_t msg = {
+			// VIBRATION
-				.time_usec = est.timestamp,
+			mavlink_vibration_t msg = {};
-				.vibration_x = est.vibe[0],
+			msg.time_usec = est.timestamp;
-				.vibration_y = est.vibe[1],
+			msg.vibration_x = est.vibe[0];
-				.vibration_z = est.vibe[2]
+			msg.vibration_y = est.vibe[1];
-			};
+			msg.vibration_z = est.vibe[2];
-
 			mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;
@@ -2118,7 +2139,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct att_pos_mocap_s mocap;
+		att_pos_mocap_s mocap;
 
 		if (_mocap_sub->update(&_mocap_time, &mocap)) {
 			mavlink_att_pos_mocap_t msg = {};
@@ -2192,7 +2213,7 @@ protected:
 		/* we're sending the GPS home periodically to ensure the
 		 * the GCS does pick it up at one point */
 		if (_home_sub->is_published()) {
-			struct home_position_s home;
+			home_position_s home;
 
 			if (_home_sub->update(&home)) {
 				if (home.valid_hpos) {
@@ -2284,15 +2305,13 @@ private:
 
 protected:
 	explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_act_sub(nullptr),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N)),
 		_act_time(0)
-	{
+	{}
-		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
-	}
 
 	bool send(const hrt_abstime t)
 	{
-		struct actuator_outputs_s act;
+		actuator_outputs_s act;
 
 		if (_act_sub->update(&_act_time, &act)) {
 			mavlink_servo_output_raw_t msg = {};
@@ -2398,7 +2417,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct actuator_controls_s att_ctrl;
+		actuator_controls_s att_ctrl;
 
 		if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) {
 			mavlink_actuator_control_target_t msg = {};
@@ -2475,10 +2494,11 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		vehicle_status_s status = {};
-		struct actuator_outputs_s act;
+		actuator_outputs_s act = {};
 
-		bool updated = _act_sub->update(&_act_time, &act);
+		bool updated = false;
+		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _status_sub->update(&_status_time, &status);
 
 		if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
@@ -2621,7 +2641,6 @@ public:
 
 private:
 	MavlinkOrbSubscription *_status_sub;
-	uint64_t _status_time;
 
 	MavlinkOrbSubscription *_act_sub;
 	uint64_t _act_time;
@@ -2633,116 +2652,118 @@ private:
 protected:
 	explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
-		_status_time(0),
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
 		_act_time(0)
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		actuator_outputs_s act;
-		struct actuator_outputs_s act;
 
-		bool updated = _act_sub->update(&_act_time, &act);
+		if (_act_sub->update(&_act_time, &act)) {
-		updated |= _status_sub->update(&_status_time, &status);
+			vehicle_status_s status = {};
+			_status_sub->update(&status);
 
-		if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			if ((status.timestamp > 0) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
-			/* translate the current syste state to mavlink state and mode */
+				/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state;
+				uint8_t mavlink_state;
-			uint8_t mavlink_base_mode;
+				uint8_t mavlink_base_mode;
-			uint32_t mavlink_custom_mode;
+				uint32_t mavlink_custom_mode;
-			mavlink_hil_actuator_controls_t msg = {};
+				mavlink_hil_actuator_controls_t msg = {};
 
-			get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+				get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
-			const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
+				const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
 
-			unsigned system_type = _mavlink->get_system_type();
+				unsigned system_type = _mavlink->get_system_type();
 
-			/* scale outputs depending on system type */
+				/* scale outputs depending on system type */
-			if (system_type == MAV_TYPE_QUADROTOR ||
+				if (system_type == MAV_TYPE_QUADROTOR ||
-			    system_type == MAV_TYPE_HEXAROTOR ||
+				    system_type == MAV_TYPE_HEXAROTOR ||
-			    system_type == MAV_TYPE_OCTOROTOR ||
+				    system_type == MAV_TYPE_OCTOROTOR ||
-			    system_type == MAV_TYPE_VTOL_DUOROTOR ||
+				    system_type == MAV_TYPE_VTOL_DUOROTOR ||
-			    system_type == MAV_TYPE_VTOL_QUADROTOR ||
+				    system_type == MAV_TYPE_VTOL_QUADROTOR ||
-			    system_type == MAV_TYPE_VTOL_RESERVED2) {
+				    system_type == MAV_TYPE_VTOL_RESERVED2) {
 
-				/* multirotors: set number of rotor outputs depending on type */
+					/* multirotors: set number of rotor outputs depending on type */
 
-				unsigned n;
+					unsigned n;
 
-				switch (system_type) {
+					switch (system_type) {
-				case MAV_TYPE_QUADROTOR:
+					case MAV_TYPE_QUADROTOR:
-					n = 4;
+						n = 4;
-					break;
+						break;
 
-				case MAV_TYPE_HEXAROTOR:
+					case MAV_TYPE_HEXAROTOR:
-					n = 6;
+						n = 6;
-					break;
+						break;
 
-				case MAV_TYPE_VTOL_DUOROTOR:
+					case MAV_TYPE_VTOL_DUOROTOR:
-					n = 2;
+						n = 2;
-					break;
+						break;
 
-				case MAV_TYPE_VTOL_QUADROTOR:
+					case MAV_TYPE_VTOL_QUADROTOR:
-					n = 4;
+						n = 4;
-					break;
+						break;
 
-				case MAV_TYPE_VTOL_RESERVED2:
+					case MAV_TYPE_VTOL_RESERVED2:
-					n = 8;
+						n = 8;
-					break;
+						break;
 
-				default:
+					default:
-					n = 8;
+						n = 8;
-					break;
+						break;
-				}
+					}
 
-				for (unsigned i = 0; i < 16; i++) {
+					for (unsigned i = 0; i < 16; i++) {
-					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-						if (i < n) {
+							if (i < n) {
-							/* scale PWM out 900..2100 us to 0..1 for rotors */
+								/* scale PWM out 900..2100 us to 0..1 for rotors */
-							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+
+							} else {
+								/* scale PWM out 900..2100 us to -1..1 for other channels */
+								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+							}
 
 						} else {
-							/* scale PWM out 900..2100 us to -1..1 for other channels */
+							/* send 0 when disarmed and for disabled channels */
-							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+							msg.controls[i] = 0.0f;
 						}
+					}
 
-					} else {
+				} else {
-						/* send 0 when disarmed and for disabled channels */
+					/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
-						msg.controls[i] = 0.0f;
+
+					for (unsigned i = 0; i < 16; i++) {
+						if (act.output[i] > PWM_DEFAULT_MIN / 2) {
+							if (i != 3) {
+								/* scale PWM out 900..2100 us to -1..1 for normal channels */
+								msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+
+							} else {
+								/* scale PWM out 900..2100 us to 0..1 for throttle */
+								msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+							}
+
+						} else {
+							/* set 0 for disabled channels */
+							msg.controls[i] = 0.0f;
+						}
 					}
 				}
 
-			} else {
+				msg.time_usec = hrt_absolute_time();
-				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
+				msg.mode = mavlink_base_mode;
+				msg.flags = 0;
 
-				for (unsigned i = 0; i < 16; i++) {
+				mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
-					if (act.output[i] > PWM_DEFAULT_MIN / 2) {
-						if (i != 3) {
-							/* scale PWM out 900..2100 us to -1..1 for normal channels */
-							msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
-						} else {
+				return true;
-							/* scale PWM out 900..2100 us to 0..1 for throttle */
-							msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-						}
-
-					} else {
-						/* set 0 for disabled channels */
-						msg.controls[i] = 0.0f;
-					}
-				}
 			}
-
-			msg.time_usec = hrt_absolute_time();
-			msg.mode = mavlink_base_mode;
-			msg.flags = 0;
-
-			mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
 		}
 
-		return (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED));
+		return false;
 	}
 };
 
@@ -2781,6 +2802,7 @@ public:
 
 private:
 	MavlinkOrbSubscription *_pos_sp_triplet_sub;
+	uint64_t _pos_sp_triplet_timestamp{0};
 
 	/* do not allow top copying this class */
 	MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
@@ -2793,9 +2815,9 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct position_setpoint_triplet_s pos_sp_triplet;
+		position_setpoint_triplet_s pos_sp_triplet;
 
-		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+		if (_pos_sp_triplet_sub->update(&_pos_sp_triplet_timestamp, &pos_sp_triplet)) {
 			mavlink_position_target_global_int_t msg = {};
 
 			msg.time_boot_ms = hrt_absolute_time() / 1000;
@@ -2863,7 +2885,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_local_position_setpoint_s pos_sp;
+		vehicle_local_position_setpoint_s pos_sp;
 
 		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
 			mavlink_position_target_local_ned_t msg = {};
@@ -2927,8 +2949,8 @@ public:
 private:
 	MavlinkOrbSubscription *_att_sp_sub;
 	MavlinkOrbSubscription *_att_rates_sp_sub;
+
 	uint64_t _att_sp_time;
-	uint64_t _att_rates_sp_time;
 
 	/* do not allow top copying this class */
 	MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
@@ -2938,18 +2960,17 @@ protected:
 	explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
 		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
-		_att_sp_time(0),
+		_att_sp_time(0)
-		_att_rates_sp_time(0)
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_attitude_setpoint_s att_sp;
+		vehicle_attitude_setpoint_s att_sp;
 
 		if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
 
-			struct vehicle_rates_setpoint_s att_rates_sp = {};
+			vehicle_rates_setpoint_s att_rates_sp = {};
-			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
+			_att_rates_sp_sub->update(&att_rates_sp);
 
 			mavlink_attitude_target_t msg = {};
 
@@ -3027,7 +3048,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct rc_input_values rc;
+		rc_input_values rc;
 
 		if (_rc_sub->update(&_rc_time, &rc)) {
 
@@ -3132,7 +3153,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct manual_control_setpoint_s manual;
+		manual_control_setpoint_s manual;
 
 		if (_manual_sub->update(&_manual_time, &manual)) {
 			mavlink_manual_control_t msg = {};
@@ -3209,7 +3230,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct optical_flow_s flow;
+		optical_flow_s flow;
 
 		if (_flow_sub->update(&_flow_time, &flow)) {
 			mavlink_optical_flow_rad_t msg = {};
@@ -3482,6 +3503,9 @@ private:
 	MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
 	MavlinkOrbSubscription *_tecs_status_sub;
 
+	uint64_t _fw_pos_ctrl_status_timestamp{0};
+	uint64_t _tecs_status_timestamp{0};
+
 	/* do not allow top copying this class */
 	MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &);
 	MavlinkStreamNavControllerOutput &operator = (const MavlinkStreamNavControllerOutput &);
@@ -3494,13 +3518,14 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct fw_pos_ctrl_status_s _fw_pos_ctrl_status;
+		fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
-		struct tecs_status_s _tecs_status;
+		tecs_status_s _tecs_status = {};
 
-		const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status);
+		bool updated = false;
-		const bool updated_tecs = _tecs_status_sub->update(&_tecs_status);
+		updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_timestamp, &_fw_pos_ctrl_status);
+		updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &_tecs_status);
 
-		if (updated_fw_pos_ctrl_status || updated_tecs) {
+		if (updated) {
 			mavlink_nav_controller_output_t msg = {};
 
 			msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
@@ -3568,7 +3593,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		vehicle_status_s status;
 
 		if (_status_sub->update(&status)) {
 
@@ -3644,7 +3669,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct distance_sensor_s dist_sensor;
+		distance_sensor_s dist_sensor;
 
 		if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
 
@@ -3746,10 +3771,10 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
-		struct vehicle_land_detected_s land_detected;
 		bool updated = false;
 
+		vehicle_status_s status;
+
 		if (_status_sub->update(&status)) {
 			updated = true;
 
@@ -3769,6 +3794,8 @@ protected:
 			}
 		}
 
+		vehicle_land_detected_s land_detected;
+
 		if (_landed_sub->update(&land_detected)) {
 			updated = true;
 
@@ -3778,8 +3805,8 @@ protected:
 			} else if (!land_detected.landed) {
 				_msg.landed_state = MAV_LANDED_STATE_IN_AIR;
 
-				vehicle_control_mode_s control_mode = {};
+				vehicle_control_mode_s control_mode;
-				position_setpoint_triplet_s pos_sp_triplet = {};
+				position_setpoint_triplet_s pos_sp_triplet;
 
 				if (_control_mode_sub->update(&control_mode) && _pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 					if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
@@ -3836,7 +3863,6 @@ public:
 	}
 
 private:
-	MavlinkOrbSubscription *_global_pos_sub;
 	MavlinkOrbSubscription *_local_pos_sub;
 	MavlinkOrbSubscription *_home_sub;
 	MavlinkOrbSubscription *_sensor_sub;
@@ -3849,7 +3875,6 @@ private:
 
 protected:
 	explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
 		_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
 		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
 		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
@@ -3857,7 +3882,7 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		mavlink_altitude_t msg;
+		mavlink_altitude_t msg = {};
 
 		msg.altitude_monotonic = NAN;
 		msg.altitude_amsl = NAN;
@@ -3866,20 +3891,20 @@ protected:
 		msg.altitude_terrain = NAN;
 		msg.bottom_clearance = NAN;
 
-		bool updated = false;
-
 		// always update monotonic altitude
+		bool sensor_updated = false;
 		sensor_combined_s sensor = {};
-		uint64_t sensor_time = 0;
+		_sensor_sub->update(&sensor);
-		_sensor_sub->update(&sensor_time, &sensor);
 
-		if ((sensor.timestamp > 0) && (sensor_time > 0)) {
+		if (sensor.timestamp > 0) {
 			msg.altitude_monotonic = sensor.baro_alt_meter;
 
-			updated = true;
+			sensor_updated = true;
 		}
 
-		vehicle_local_position_s local_pos = {};
+		bool lpos_updated = false;
+
+		vehicle_local_position_s local_pos;
 
 		if (_local_pos_sub->update(&_local_pos_time, &local_pos)) {
 
@@ -3893,7 +3918,7 @@ protected:
 
 				msg.altitude_local = -local_pos.z;
 
-				home_position_s home;
+				home_position_s home = {};
 				_home_sub->update(&home);
 
 				if (home.valid_alt) {
@@ -3909,15 +3934,21 @@ protected:
 				}
 			}
 
-			updated = true;
+			lpos_updated = true;
 		}
 
-		if (updated) {
+		// local position timeout after 10 ms
+		// avoid publishing only baro altitude_monotonic if possible
+		bool lpos_timeout = (hrt_elapsed_time(&_local_pos_time) > 10000);
+
+		if (lpos_updated || (sensor_updated && lpos_timeout)) {
 			msg.time_usec = hrt_absolute_time();
 			mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
 
-		return updated;
+		return false;
 	}
 };
 
@@ -3958,8 +3989,7 @@ private:
 	MavlinkOrbSubscription *_wind_estimate_sub;
 	uint64_t _wind_estimate_time;
 
-	MavlinkOrbSubscription *_global_pos_sub;
+	MavlinkOrbSubscription *_local_pos_sub;
-	uint64_t _global_pos_time;
 
 	/* do not allow top copying this class */
 	MavlinkStreamWind(MavlinkStreamWind &);
@@ -3969,13 +3999,12 @@ protected:
 	explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
 		_wind_estimate_time(0),
-		_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)))
-		_global_pos_time(0)
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		struct wind_estimate_s wind_estimate;
+		wind_estimate_s wind_estimate;
 
 		if (_wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate)) {
 
@@ -3990,10 +4019,9 @@ protected:
 			msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east;
 			msg.var_vert = 0.0f;
 
-			struct vehicle_global_position_s global_pos = {};
+			vehicle_local_position_s lpos = {};
-			_global_pos_sub->update(&_global_pos_time, &global_pos);
+			_local_pos_sub->update(&lpos);
-			msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN;
+			msg.wind_alt = (lpos.z_valid && lpos.z_global) ? (-lpos.z + lpos.ref_alt) : NAN;
-
 
 			msg.horiz_accuracy = 0.0f;
 			msg.vert_accuracy = 0.0f;
@@ -4192,20 +4220,20 @@ protected:
 
 	bool send(const hrt_abstime t)
 	{
-		struct actuator_controls_s actuator = {};
+		actuator_controls_s actuator = {};
-		struct airspeed_s airspeed = {};
+		airspeed_s airspeed = {};
-		struct battery_status_s battery = {};
+		battery_status_s battery = {};
-		struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {};
+		fw_pos_ctrl_status_s fw_pos_ctrl_status = {};
-		struct home_position_s home = {};
+		home_position_s home = {};
-		struct mission_result_s mission_result = {};
+		mission_result_s mission_result = {};
-		struct sensor_combined_s sensor = {};
+		sensor_combined_s sensor = {};
-		struct tecs_status_s tecs_status = {};
+		tecs_status_s tecs_status = {};
-		struct vehicle_attitude_s attitude = {};
+		vehicle_attitude_s attitude = {};
-		struct vehicle_attitude_setpoint_s attitude_sp = {};
+		vehicle_attitude_setpoint_s attitude_sp = {};
-		struct vehicle_global_position_s global_pos = {};
+		vehicle_global_position_s global_pos = {};
-		struct vehicle_gps_position_s gps = {};
+		vehicle_gps_position_s gps = {};
-		struct vehicle_land_detected_s land_detected = {};
+		vehicle_land_detected_s land_detected = {};
-		struct vehicle_status_s status = {};
+		vehicle_status_s status = {};
 
 		bool updated = _status_sub->update(&_status_time, &status);
 		updated |= _actuator_sub->update(&_actuator_time, &actuator);
@@ -4322,8 +4350,6 @@ private:
 	MavlinkOrbSubscription *_gpos_sub;
 	uint64_t _att_time;
 	uint64_t _gpos_time;
-	struct vehicle_attitude_s _att;
-	struct vehicle_global_position_s _gpos;
 
 	/* do not allow top copying this class */
 	MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &);
@@ -4334,43 +4360,42 @@ protected:
 		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))),
 		_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))),
 		_att_time(0),
-		_gpos_time(0),
+		_gpos_time(0)
-		_att(),
-		_gpos()
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		bool att_updated = _att_sub->update(&_att_time, &_att);
+
-		bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos);
+		vehicle_attitude_s att = {};
+		vehicle_global_position_s gpos = {};
+		bool att_updated = _att_sub->update(&_att_time, &att);
+		bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos);
 
 		if (att_updated || gpos_updated) {
 
 			mavlink_hil_state_quaternion_t msg = {};
 
-			if (att_updated) {
+			// vehicle_attitude -> hil_state_quaternion
-				msg.attitude_quaternion[0] = _att.q[0];
+			msg.attitude_quaternion[0] = att.q[0];
-				msg.attitude_quaternion[1] = _att.q[1];
+			msg.attitude_quaternion[1] = att.q[1];
-				msg.attitude_quaternion[2] = _att.q[2];
+			msg.attitude_quaternion[2] = att.q[2];
-				msg.attitude_quaternion[3] = _att.q[3];
+			msg.attitude_quaternion[3] = att.q[3];
-				msg.rollspeed = _att.rollspeed;
+			msg.rollspeed = att.rollspeed;
-				msg.pitchspeed = _att.pitchspeed;
+			msg.pitchspeed = att.pitchspeed;
-				msg.yawspeed = _att.yawspeed;
+			msg.yawspeed = att.yawspeed;
-			}
 
-			if (gpos_updated) {
+			// vehicle_global_position -> hil_state_quaternion
-				msg.lat = _gpos.lat;
+			msg.lat = gpos.lat;
-				msg.lon = _gpos.lon;
+			msg.lon = gpos.lon;
-				msg.alt = _gpos.alt;
+			msg.alt = gpos.alt;
-				msg.vx = _gpos.vel_n;
+			msg.vx = gpos.vel_n;
-				msg.vy = _gpos.vel_e;
+			msg.vy = gpos.vel_e;
-				msg.vz = _gpos.vel_d;
+			msg.vz = gpos.vel_d;
-				msg.ind_airspeed = 0;
+			msg.ind_airspeed = 0;
-				msg.true_airspeed = 0;
+			msg.true_airspeed = 0;
-				msg.xacc = 0;
+			msg.xacc = 0;
-				msg.yacc = 0;
+			msg.yacc = 0;
-				msg.zacc = 0;
+			msg.zacc = 0;
-			}
 
 			mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -114,12 +114,7 @@ MavlinkOrbSubscription::update(void *data)
 		return false;
 	}
 
-	if (orb_copy(_topic, _fd, data)) {
+	if (orb_copy(_topic, _fd, data) != PX4_OK) {
-		if (data != nullptr) {
-			/* error copying topic data */
-			memset(data, 0, _topic->o_size);
-		}
-
 		return false;
 	}