diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6781f64a7b..c7cb997b8b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(_mag_missed_perf); + perf_free(_msg_missed_imu_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); - - if (_device_id_mag != 0) { - perf_print_counter(_mag_missed_perf); - } + perf_print_counter(_msg_missed_imu_perf); + perf_print_counter(_msg_missed_air_data_perf); + perf_print_counter(_msg_missed_airspeed_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); - PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation, - _vehicle_imu_sub.get_last_generation()); + perf_count(_msg_missed_imu_perf); } 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 ×tamp) // 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) { - PX4_ERR("%d - landing_target_pose lost, generation %d -> %d", _instance, last_generation, - _landing_target_pose_sub.get_last_generation()); + } else if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_landing_target_pose_perf); } // 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) { - PX4_ERR("%d - vehicle_visual_odometry lost, generation %d -> %d", _instance, last_generation, - _ev_odom_sub.get_last_generation()); + } else if (_ev_odom_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_odometry_perf); } - 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 (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; + if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) { + ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD; - if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) { - ev_data.vel_frame = velocity_frame_t::BODY_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() * param_evv_noise_var; - } + } else { + ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD; } - // 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; + // velocity measurement error from ev_data or parameters + float param_evv_noise_var = sq(_param_ekf2_evv_noise.get()); - 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 - 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); - } + } else { + ev_data.velCov = matrix::eye() * param_evv_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) { - PX4_ERR("%d - optical_flow lost, generation %d -> %d", _instance, last_generation, - _optical_flow_sub.get_last_generation()); + } else if (_optical_flow_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_optical_flow_perf); } - 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 { - .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, - }; + if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && + PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + flow.dt < 1) { - if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && - PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && - flow.dt < 1) { + // Save sensor limits reported by the optical flow sensor + _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, + optical_flow.max_ground_distance); - // Save sensor limits reported by the optical flow sensor - _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, - optical_flow.max_ground_distance); + _ekf.setOpticalFlowData(flow); - _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) { - vehicle_gps_position_s vehicle_gps_position; + 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)) { - 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); + 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"); - _gps_time_usec = gps_msg.time_usec; - _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid; + } 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, + .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) { - perf_count(_mag_missed_perf); - PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation, - _magnetometer_sub.get_last_generation()); + } else if (_magnetometer_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_magnetometer_perf); } 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_subs{ORB_ID::distance_sensor}; - for (unsigned i = 0; i < distance_sensor_subs.size(); i++) { - distance_sensor_s distance_sensor; + if (distance_sensor_subs.advertised()) { + for (unsigned i = 0; i < distance_sensor_subs.size(); i++) { + distance_sensor_s distance_sensor; - if (distance_sensor_subs[i].copy(&distance_sensor)) { - // only use the first instace which has the correct orientation - if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) - && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { + if (distance_sensor_subs[i].copy(&distance_sensor)) { + // only use the first instace which has the correct orientation + if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) + && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { - if (_distance_sensor_sub.ChangeInstance(i)) { - PX4_INFO("%d - selected distance_sensor:%d", _instance, i); - _distance_sensor_selected = true; + 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) { - PX4_ERR("%d - distance_sensor lost, generation %d -> %d", _instance, last_generation, - _distance_sensor_sub.get_last_generation()); + } else if (_distance_sensor_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_distance_sensor_perf); } - 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) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 28711f24b5..3c201f1979 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/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 _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag message missed")}; + perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU 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)