ekf2: update message lost error messages to perf counters

- only allocate these perf counters if data source is present

src/modules/ekf2/EKF2.cpp

@@ -169,8 +169,15 @@ EKF2::~EKF2()
 {
 	perf_free(_ecl_ekf_update_perf);
 	perf_free(_ecl_ekf_update_full_perf);
-	perf_free(_imu_missed_perf);
+	perf_free(_msg_missed_imu_perf);
-	perf_free(_mag_missed_perf);
+	perf_free(_msg_missed_air_data_perf);
+	perf_free(_msg_missed_airspeed_perf);
+	perf_free(_msg_missed_distance_sensor_perf);
+	perf_free(_msg_missed_gps_perf);
+	perf_free(_msg_missed_landing_target_pose_perf);
+	perf_free(_msg_missed_magnetometer_perf);
+	perf_free(_msg_missed_odometry_perf);
+	perf_free(_msg_missed_optical_flow_perf);
 }
 
 bool EKF2::multi_init(int imu, int mag)
@@ -222,11 +229,15 @@ int EKF2::print_status()
 		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
-	perf_print_counter(_imu_missed_perf);
+	perf_print_counter(_msg_missed_imu_perf);
-
+	perf_print_counter(_msg_missed_air_data_perf);
-	if (_device_id_mag != 0) {
+	perf_print_counter(_msg_missed_airspeed_perf);
-		perf_print_counter(_mag_missed_perf);
+	perf_print_counter(_msg_missed_distance_sensor_perf);
-	}
+	perf_print_counter(_msg_missed_gps_perf);
+	perf_print_counter(_msg_missed_landing_target_pose_perf);
+	perf_print_counter(_msg_missed_magnetometer_perf);
+	perf_print_counter(_msg_missed_odometry_perf);
+	perf_print_counter(_msg_missed_optical_flow_perf);
 
 	return 0;
 }
@@ -307,9 +318,7 @@ void EKF2::Run()
 		imu_updated = _vehicle_imu_sub.update(&imu);
 
 		if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
-			perf_count(_imu_missed_perf);
+			perf_count(_msg_missed_imu_perf);
-			PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
-				  _vehicle_imu_sub.get_last_generation());
 		}
 
 		imu_sample_new.time_us = imu.timestamp_sample;
@@ -344,9 +353,14 @@ void EKF2::Run()
 		}
 
 	} else {
+		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
 		sensor_combined_s sensor_combined;
 		imu_updated = _sensor_combined_sub.update(&sensor_combined);
 
+		if (imu_updated && (_sensor_combined_sub.get_last_generation() != last_generation + 1)) {
+			perf_count(_msg_missed_imu_perf);
+		}
+
 		imu_sample_new.time_us = sensor_combined.timestamp;
 		imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
 		imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
@@ -1019,7 +1033,7 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 	// publish estimator states
 	estimator_states_s states;
 	states.timestamp_sample = timestamp;
-	states.n_states = 24;
+	states.n_states = Ekf::_k_num_states;
 	_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
 	_ekf.covariances_diagonal().copyTo(states.covariances);
 	states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
@@ -1263,9 +1277,17 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF airspeed sample
+	const unsigned last_generation = _airspeed_sub.get_last_generation();
 	airspeed_s airspeed;
 
 	if (_airspeed_sub.update(&airspeed)) {
+		if (_msg_missed_airspeed_perf == nullptr) {
+			_msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed");
+
+		} else if (_airspeed_sub.get_last_generation() != last_generation + 1) {
+			perf_count(_msg_missed_airspeed_perf);
+		}
+
 		// The airspeed measurement received via the airspeed.msg topic has not been corrected
 		// for scale favtor errors and requires the ASPD_SCALE correction to be applied.
 		// This could be avoided if true_airspeed_m_s from the airspeed-validated.msg topic
@@ -1297,10 +1319,11 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 	landing_target_pose_s landing_target_pose;
 
 	if (_landing_target_pose_sub.update(&landing_target_pose)) {
+		if (_msg_missed_landing_target_pose_perf == nullptr) {
+			_msg_missed_landing_target_pose_perf = perf_alloc(PC_COUNT, MODULE_NAME": landing_target_pose messages missed");
 
-		if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) {
+		} else if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - landing_target_pose lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_msg_missed_landing_target_pose_perf);
-				_landing_target_pose_sub.get_last_generation());
 		}
 
 		// we can only use the landing target if it has a fixed position and  a valid velocity estimate
@@ -1319,9 +1342,17 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF baro sample
+	const unsigned last_generation = _airdata_sub.get_last_generation();
 	vehicle_air_data_s airdata;
 
 	if (_airdata_sub.update(&airdata)) {
+		if (_msg_missed_air_data_perf == nullptr) {
+			_msg_missed_air_data_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_air_data messages missed");
+
+		} else if (_airdata_sub.get_last_generation() != last_generation + 1) {
+			perf_count(_msg_missed_air_data_perf);
+		}
+
 		_ekf.set_air_density(airdata.rho);
 
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
@@ -1335,97 +1366,95 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 
 bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
 {
+	// EKF external vision sample
 	bool new_ev_odom = false;
 	const unsigned last_generation = _ev_odom_sub.get_last_generation();
 
-	// EKF external vision sample
 	if (_ev_odom_sub.update(&ev_odom)) {
+		if (_msg_missed_odometry_perf == nullptr) {
+			_msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed");
 
-		if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
+		} else if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - vehicle_visual_odometry lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_msg_missed_odometry_perf);
-				_ev_odom_sub.get_last_generation());
 		}
 
-		if (_param_ekf2_aid_mask.get() & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
+		extVisionSample ev_data{};
 
-			extVisionSample ev_data{};
+		// if error estimates are unavailable, use parameter defined defaults
 
-			// if error estimates are unavailable, use parameter defined defaults
+		// check for valid velocity data
+		if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
+			ev_data.vel(0) = ev_odom.vx;
+			ev_data.vel(1) = ev_odom.vy;
+			ev_data.vel(2) = ev_odom.vz;
 
-			// check for valid velocity data
+			if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
-			if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
+				ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
-				ev_data.vel(0) = ev_odom.vx;
-				ev_data.vel(1) = ev_odom.vy;
-				ev_data.vel(2) = ev_odom.vz;
 
-				if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
+			} else {
-					ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+				ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
-
-				} else {
-					ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
-				}
-
-				// velocity measurement error from ev_data or parameters
-				float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
-
-				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
-				    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
-				    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
-					ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
-					ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
-					ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
-					ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
-					ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
-					ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
-
-				} else {
-					ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
-				}
 			}
 
-			// check for valid position data
+			// velocity measurement error from ev_data or parameters
-			if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
+			float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
-				ev_data.pos(0) = ev_odom.x;
-				ev_data.pos(1) = ev_odom.y;
-				ev_data.pos(2) = ev_odom.z;
 
-				float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
+			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
+			    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
+			    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
+				ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
+				ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
+				ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
+				ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
+				ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
+				ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
 
-				// position measurement error from ev_data or parameters
+			} else {
-				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
+				ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
-				    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
-				    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
-					ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
-					ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
-					ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
-
-				} else {
-					ev_data.posVar.setAll(param_evp_noise_var);
-				}
 			}
-
-			// check for valid orientation data
-			if (PX4_ISFINITE(ev_odom.q[0])) {
-				ev_data.quat = Quatf(ev_odom.q);
-
-				// orientation measurement error from ev_data or parameters
-				float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
-
-				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
-					ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
-
-				} else {
-					ev_data.angVar = param_eva_noise_var;
-				}
-			}
-
-			// use timestamp from external computer, clocks are synchronized when using MAVROS
-			ev_data.time_us = ev_odom.timestamp_sample;
-			_ekf.setExtVisionData(ev_data);
-
-			new_ev_odom = true;
 		}
 
+		// check for valid position data
+		if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
+			ev_data.pos(0) = ev_odom.x;
+			ev_data.pos(1) = ev_odom.y;
+			ev_data.pos(2) = ev_odom.z;
+
+			float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
+
+			// position measurement error from ev_data or parameters
+			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
+			    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
+			    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
+				ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
+				ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
+				ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
+
+			} else {
+				ev_data.posVar.setAll(param_evp_noise_var);
+			}
+		}
+
+		// check for valid orientation data
+		if (PX4_ISFINITE(ev_odom.q[0])) {
+			ev_data.quat = Quatf(ev_odom.q);
+
+			// orientation measurement error from ev_data or parameters
+			float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
+
+			if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
+				ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
+
+			} else {
+				ev_data.angVar = param_eva_noise_var;
+			}
+		}
+
+		// use timestamp from external computer, clocks are synchronized when using MAVROS
+		ev_data.time_us = ev_odom.timestamp_sample;
+		_ekf.setExtVisionData(ev_data);
+
+		new_ev_odom = true;
+
 		ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
@@ -1435,40 +1464,39 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 
 bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow)
 {
+	// EKF flow sample
 	bool new_optical_flow = false;
 	const unsigned last_generation = _optical_flow_sub.get_last_generation();
 
 	if (_optical_flow_sub.update(&optical_flow)) {
+		if (_msg_missed_optical_flow_perf == nullptr) {
+			_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
 
-		if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
+		} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - optical_flow lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_msg_missed_optical_flow_perf);
-				_optical_flow_sub.get_last_generation());
 		}
 
-		if (_param_ekf2_aid_mask.get() & MASK_USE_OF) {
+		flowSample flow {
+			.time_us = optical_flow.timestamp,
+			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
+			// is produced by a RH rotation of the image about the sensor axis.
+			.flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
+			.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
+			.dt = 1e-6f * (float)optical_flow.integration_timespan,
+			.quality = optical_flow.quality,
+		};
 
-			flowSample flow {
+		if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
-				.time_us = optical_flow.timestamp,
+		    PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
-				// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
+		    flow.dt < 1) {
-				// is produced by a RH rotation of the image about the sensor axis.
-				.flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
-				.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
-				.dt = 1e-6f * (float)optical_flow.integration_timespan,
-				.quality = optical_flow.quality,
-			};
 
-			if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
+			// Save sensor limits reported by the optical flow sensor
-			    PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
+			_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
-			    flow.dt < 1) {
+						     optical_flow.max_ground_distance);
 
-				// Save sensor limits reported by the optical flow sensor
+			_ekf.setOpticalFlowData(flow);
-				_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
-							     optical_flow.max_ground_distance);
 
-				_ekf.setOpticalFlowData(flow);
+			new_optical_flow = true;
-
-				new_optical_flow = true;
-			}
 		}
 
 		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
@@ -1481,37 +1509,43 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
 void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF GPS message
-	if (_param_ekf2_aid_mask.get() & MASK_USE_GPS) {
+	const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation();
-		vehicle_gps_position_s vehicle_gps_position;
+	vehicle_gps_position_s vehicle_gps_position;
 
-		if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+	if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-			gps_message gps_msg{
+		if (_msg_missed_gps_perf == nullptr) {
-				.time_usec = vehicle_gps_position.timestamp,
+			_msg_missed_gps_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gps_position messages missed");
-				.lat = vehicle_gps_position.lat,
-				.lon = vehicle_gps_position.lon,
-				.alt = vehicle_gps_position.alt,
-				.yaw = vehicle_gps_position.heading,
-				.yaw_offset = vehicle_gps_position.heading_offset,
-				.fix_type = vehicle_gps_position.fix_type,
-				.eph = vehicle_gps_position.eph,
-				.epv = vehicle_gps_position.epv,
-				.sacc = vehicle_gps_position.s_variance_m_s,
-				.vel_m_s = vehicle_gps_position.vel_m_s,
-				.vel_ned = Vector3f{
-					vehicle_gps_position.vel_n_m_s,
-					vehicle_gps_position.vel_e_m_s,
-					vehicle_gps_position.vel_d_m_s
-				},
-				.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
-				.nsats = vehicle_gps_position.satellites_used,
-				.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
-					      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
-			};
-			_ekf.setGpsData(gps_msg);
 
-			_gps_time_usec = gps_msg.time_usec;
+		} else if (_vehicle_gps_position_sub.get_last_generation() != last_generation + 1) {
-			_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
+			perf_count(_msg_missed_gps_perf);
 		}
+
+		gps_message gps_msg{
+			.time_usec = vehicle_gps_position.timestamp,
+			.lat = vehicle_gps_position.lat,
+			.lon = vehicle_gps_position.lon,
+			.alt = vehicle_gps_position.alt,
+			.yaw = vehicle_gps_position.heading,
+			.yaw_offset = vehicle_gps_position.heading_offset,
+			.fix_type = vehicle_gps_position.fix_type,
+			.eph = vehicle_gps_position.eph,
+			.epv = vehicle_gps_position.epv,
+			.sacc = vehicle_gps_position.s_variance_m_s,
+			.vel_m_s = vehicle_gps_position.vel_m_s,
+			.vel_ned = Vector3f{
+				vehicle_gps_position.vel_n_m_s,
+				vehicle_gps_position.vel_e_m_s,
+				vehicle_gps_position.vel_d_m_s
+			},
+			.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
+			.nsats = vehicle_gps_position.satellites_used,
+			.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
+				      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
+		};
+		_ekf.setGpsData(gps_msg);
+
+		_gps_time_usec = gps_msg.time_usec;
+		_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
 	}
 }
 
@@ -1521,11 +1555,11 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 	vehicle_magnetometer_s magnetometer;
 
 	if (_magnetometer_sub.update(&magnetometer)) {
+		if (_msg_missed_magnetometer_perf == nullptr) {
+			_msg_missed_magnetometer_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_magnetometer messages missed");
 
-		if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
+		} else if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
-			perf_count(_mag_missed_perf);
+			perf_count(_msg_missed_magnetometer_perf);
-			PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
-				  _magnetometer_sub.get_last_generation());
 		}
 
 		bool reset = false;
@@ -1568,17 +1602,20 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 		// get subscription index of first downward-facing range sensor
 		uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};
 
-		for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
+		if (distance_sensor_subs.advertised()) {
-			distance_sensor_s distance_sensor;
+			for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
+				distance_sensor_s distance_sensor;
 
-			if (distance_sensor_subs[i].copy(&distance_sensor)) {
+				if (distance_sensor_subs[i].copy(&distance_sensor)) {
-				// only use the first instace which has the correct orientation
+					// only use the first instace which has the correct orientation
-				if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
+					if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
-				    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
+					    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
 
-					if (_distance_sensor_sub.ChangeInstance(i)) {
+						if (_distance_sensor_sub.ChangeInstance(i)) {
-						PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
+							PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
-						_distance_sensor_selected = true;
+							_distance_sensor_selected = true;
+							break;
+						}
 					}
 				}
 			}
@@ -1590,15 +1627,13 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	distance_sensor_s distance_sensor;
 
 	if (_distance_sensor_sub.update(&distance_sensor)) {
+		if (_msg_missed_distance_sensor_perf == nullptr) {
+			_msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed");
 
-		if (_distance_sensor_sub.get_last_generation() != last_generation + 1) {
+		} else if (_distance_sensor_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - distance_sensor lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_msg_missed_distance_sensor_perf);
-				_distance_sensor_sub.get_last_generation());
 		}
 
-		ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
-				(int64_t)ekf2_timestamps.timestamp / 100);
-
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
 			rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
@@ -1613,6 +1648,9 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 			_last_range_sensor_update = distance_sensor.timestamp;
 			return;
 		}
+
+		ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
+				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 
 	if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {

src/modules/ekf2/EKF2.hpp

@@ -176,8 +176,15 @@ private:
 
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
-	perf_counter_t _imu_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
+	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
-	perf_counter_t _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag message missed")};
+	perf_counter_t _msg_missed_air_data_perf{nullptr};
+	perf_counter_t _msg_missed_airspeed_perf{nullptr};
+	perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
+	perf_counter_t _msg_missed_gps_perf{nullptr};
+	perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
+	perf_counter_t _msg_missed_magnetometer_perf{nullptr};
+	perf_counter_t _msg_missed_odometry_perf{nullptr};
+	perf_counter_t _msg_missed_optical_flow_perf{nullptr};
 
 	// Used to check, save and use learned magnetometer biases
 	hrt_abstime _mag_cal_last_us{0};	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)