forked from Archive/PX4-Autopilot
ekf2: remove sensor sample uORB::Subscription missed perf counters
- these served their purpose, but are no longer useful - still worth keeping the IMU subscription missed perf count to catch any scheduling issues
This commit is contained in:
Daniel Agar
GitHub
parent
855bf8f0d2
commit
4cb293020a
|
|
@ -230,31 +230,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
|
||||
EKF2::~EKF2()
|
||||
{
|
||||
perf_free(_ecl_ekf_update_perf);
|
||||
perf_free(_ecl_ekf_update_full_perf);
|
||||
perf_free(_ekf_update_perf);
|
||||
perf_free(_msg_missed_imu_perf);
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
perf_free(_msg_missed_air_data_perf);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
perf_free(_msg_missed_airspeed_perf);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
perf_free(_msg_missed_distance_sensor_perf);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
perf_free(_msg_missed_gps_perf);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_free(_msg_missed_landing_target_pose_perf);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
perf_free(_msg_missed_magnetometer_perf);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
perf_free(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_free(_msg_missed_optical_flow_perf);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MULTI_INSTANCE)
|
||||
|
|
@ -401,31 +378,8 @@ int EKF2::print_status()
|
|||
_instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(),
|
||||
_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(_ekf_update_perf);
|
||||
perf_print_counter(_msg_missed_imu_perf);
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
perf_print_counter(_msg_missed_air_data_perf);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
perf_print_counter(_msg_missed_airspeed_perf);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
perf_print_counter(_msg_missed_distance_sensor_perf);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
perf_print_counter(_msg_missed_gps_perf);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_print_counter(_msg_missed_landing_target_pose_perf);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
perf_print_counter(_msg_missed_magnetometer_perf);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
perf_print_counter(_msg_missed_odometry_perf);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
perf_print_counter(_msg_missed_optical_flow_perf);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(DEBUG_BUILD)
|
||||
_ekf.print_status();
|
||||
|
|
@ -780,7 +734,7 @@ void EKF2::Run()
|
|||
const hrt_abstime ekf_update_start = hrt_absolute_time();
|
||||
|
||||
if (_ekf.update()) {
|
||||
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
|
||||
perf_set_elapsed(_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
|
||||
|
||||
PublishLocalPosition(now);
|
||||
PublishOdometry(now, imu_sample_new);
|
||||
|
|
@ -828,10 +782,6 @@ void EKF2::Run()
|
|||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
UpdateMagCalibration(now);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
} else {
|
||||
// ekf no update
|
||||
perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
|
||||
}
|
||||
|
||||
// publish ekf2_timestamps
|
||||
|
|
@ -2074,16 +2024,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
// EKF airspeed sample
|
||||
// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
|
||||
if (_airspeed_validated_sub.updated()) {
|
||||
const unsigned last_generation = _airspeed_validated_sub.get_last_generation();
|
||||
airspeed_validated_s airspeed_validated;
|
||||
|
||||
if (_airspeed_validated_sub.update(&airspeed_validated)) {
|
||||
if (_msg_missed_airspeed_validated_perf == nullptr) {
|
||||
_msg_missed_airspeed_validated_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed validated messages missed");
|
||||
|
||||
} else if (_airspeed_validated_sub.get_last_generation() != last_generation + 1) {
|
||||
perf_count(_msg_missed_airspeed_validated_perf);
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
|
||||
&& PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
|
||||
|
|
@ -2103,17 +2046,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
|
||||
} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
|
||||
// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
|
||||
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 ORB_ID(airspeed) topic has not been corrected
|
||||
// for scale factor errors and requires the ASPD_SCALE correction to be applied.
|
||||
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
|
||||
|
|
@ -2142,17 +2077,9 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
{
|
||||
// EKF auxiliary velocity sample
|
||||
// - use the landing target pose estimate as another source of velocity data
|
||||
const unsigned last_generation = _landing_target_pose_sub.get_last_generation();
|
||||
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");
|
||||
|
||||
} 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
|
||||
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
|
||||
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
|
||||
|
|
@ -2171,16 +2098,9 @@ 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);
|
||||
}
|
||||
|
||||
bool reset = false;
|
||||
|
||||
|
|
@ -2218,17 +2138,10 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
{
|
||||
// EKF external vision sample
|
||||
bool new_ev_odom = false;
|
||||
const unsigned last_generation = _ev_odom_sub.get_last_generation();
|
||||
|
||||
vehicle_odometry_s ev_odom;
|
||||
|
||||
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");
|
||||
|
||||
} else if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
|
||||
perf_count(_msg_missed_odometry_perf);
|
||||
}
|
||||
|
||||
extVisionSample ev_data{};
|
||||
ev_data.pos.setNaN();
|
||||
|
|
@ -2374,16 +2287,9 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
{
|
||||
// EKF flow sample
|
||||
bool new_optical_flow = false;
|
||||
const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation();
|
||||
vehicle_optical_flow_s optical_flow;
|
||||
|
||||
if (_vehicle_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");
|
||||
|
||||
} else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) {
|
||||
perf_count(_msg_missed_optical_flow_perf);
|
||||
}
|
||||
|
||||
const float dt = 1e-6f * (float)optical_flow.integration_timespan_us;
|
||||
Vector2f flow_rate;
|
||||
|
|
@ -2447,16 +2353,9 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
// EKF GPS message
|
||||
const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation();
|
||||
sensor_gps_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);
|
||||
}
|
||||
|
||||
Vector3f vel_ned;
|
||||
|
||||
|
|
@ -2498,16 +2397,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
const unsigned last_generation = _magnetometer_sub.get_last_generation();
|
||||
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");
|
||||
|
||||
} else if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
|
||||
perf_count(_msg_missed_magnetometer_perf);
|
||||
}
|
||||
|
||||
bool reset = false;
|
||||
|
||||
|
|
@ -2567,7 +2459,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
|
||||
_distance_sensor_selected = i;
|
||||
_last_range_sensor_update = distance_sensor.timestamp;
|
||||
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation() - 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -2577,17 +2468,6 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
|
||||
if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
|
||||
// EKF range sample
|
||||
|
||||
if (_msg_missed_distance_sensor_perf == nullptr) {
|
||||
_msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed");
|
||||
|
||||
} else if (_distance_sensor_subs[_distance_sensor_selected].get_last_generation() != _distance_sensor_last_generation +
|
||||
1) {
|
||||
perf_count(_msg_missed_distance_sensor_perf);
|
||||
}
|
||||
|
||||
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation();
|
||||
|
||||
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
|
||||
rangeSample range_sample {
|
||||
.time_us = distance_sensor.timestamp,
|
||||
|
|
|
|||
|
|
@ -276,10 +276,8 @@ private:
|
|||
uint64_t _start_time_us = 0; ///< system time at EKF start (uSec)
|
||||
int64_t _last_time_slip_us = 0; ///< Last time slip (uSec)
|
||||
|
||||
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 _ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": EKF update")};
|
||||
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
|
||||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
|
||||
InFlightCalibration _accel_cal{};
|
||||
InFlightCalibration _gyro_cal{};
|
||||
|
|
@ -301,8 +299,6 @@ private:
|
|||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
uint32_t _device_id_mag {0};
|
||||
|
||||
perf_counter_t _msg_missed_magnetometer_perf {nullptr};
|
||||
|
||||
// Used to control saving of mag declination to be used on next startup
|
||||
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
|
||||
|
||||
|
|
@ -341,8 +337,6 @@ private:
|
|||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
|
||||
hrt_abstime _status_aux_vel_pub_last{0};
|
||||
|
||||
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
|
@ -365,13 +359,9 @@ private:
|
|||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
hrt_abstime _status_optical_flow_pub_last{0};
|
||||
hrt_abstime _optical_flow_vel_pub_last{0};
|
||||
|
||||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
perf_counter_t _msg_missed_air_data_perf {nullptr};
|
||||
|
||||
uint8_t _baro_calibration_count {0};
|
||||
uint32_t _device_id_baro{0};
|
||||
hrt_abstime _status_baro_hgt_pub_last{0};
|
||||
|
|
@ -398,9 +388,6 @@ private:
|
|||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_airspeed_pub {ORB_ID(estimator_aid_src_airspeed)};
|
||||
hrt_abstime _status_airspeed_pub_last{0};
|
||||
|
||||
perf_counter_t _msg_missed_airspeed_perf{nullptr};
|
||||
perf_counter_t _msg_missed_airspeed_validated_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
|
|
@ -430,8 +417,6 @@ private:
|
|||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
|
||||
unsigned _distance_sensor_last_generation{0};
|
||||
perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
bool _callback_registered{false};
|
||||
|
|
@ -473,9 +458,7 @@ private:
|
|||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
perf_counter_t _msg_missed_gps_perf {nullptr};
|
||||
|
||||
uint64_t _gps_time_usec{0};
|
||||
uint64_t _gps_time_usec {0};
|
||||
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
|
||||
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
|
||||
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
|
||||
|
|
|
|||
Loading…
Reference in New Issue