diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 76abef5df6..07318add69 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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, diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 060ce108db..05b546c977 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -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_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_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_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_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