yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: update message lost error messages to perf counters

Browse Source 
- only allocate these perf counters if data source is present
This commit is contained in:
Daniel Agar
2021-06-04 20:39:45 -04:00
parent 841914462d
commit 251f1a069b
2 changed files with 204 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files

352
src/modules/ekf2/EKF2.cpp
View File

@@ -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 &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) {
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<float, 3>() * 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<float, 3>() * 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_s> 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) {

11
src/modules/ekf2/EKF2.hpp
View File

@@ -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)