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,19 +1366,18 @@ 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{};
 
 		// if error estimates are unavailable, use parameter defined defaults
@@ -1424,7 +1454,6 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 		_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,18 +1464,18 @@ 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
@@ -1469,7 +1498,6 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
 
 			new_optical_flow = true;
 		}
-		}
 
 		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
@@ -1481,10 +1509,17 @@ 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;
 
 	if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		if (_msg_missed_gps_perf == nullptr) {
+			_msg_missed_gps_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gps_position messages missed");
+
+		} else if (_vehicle_gps_position_sub.get_last_generation() != last_generation + 1) {
+			perf_count(_msg_missed_gps_perf);
+		}
+
 		gps_message gps_msg{
 			.time_usec = vehicle_gps_position.timestamp,
 			.lat = vehicle_gps_position.lat,
@@ -1513,7 +1548,6 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
 	}
 }
-}
 
 void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
@@ -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,6 +1602,7 @@ 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};
 
+		if (distance_sensor_subs.advertised()) {
 			for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
 				distance_sensor_s distance_sensor;
 
@@ -1579,6 +1614,8 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 						if (_distance_sensor_sub.ChangeInstance(i)) {
 							PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
 							_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)