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: replace vehicle_imu lost error message with perf count

Browse Source
This commit is contained in:
Daniel Agar
2021-03-31 20:33:27 -04:00
parent 00f86eb895
commit 846695f986
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -169,6 +169,7 @@ EKF2::~EKF2()
{ {
perf_free(_ecl_ekf_update_perf); perf_free(_ecl_ekf_update_perf);
perf_free(_ecl_ekf_update_full_perf); perf_free(_ecl_ekf_update_full_perf);
perf_free(_imu_missed_perf);
} }
bool EKF2::multi_init(int imu, int mag) bool EKF2::multi_init(int imu, int mag)
@@ -219,6 +220,7 @@ int EKF2::print_status()
_ekf.local_position_is_valid(), _ekf.global_position_is_valid()); _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
perf_print_counter(_ecl_ekf_update_perf); perf_print_counter(_ecl_ekf_update_perf);
perf_print_counter(_ecl_ekf_update_full_perf); perf_print_counter(_ecl_ekf_update_full_perf);
perf_print_counter(_imu_missed_perf);
return 0; return 0;
} }
@@ -298,7 +300,8 @@ void EKF2::Run()
imu_updated = _vehicle_imu_sub.update(&imu); imu_updated = _vehicle_imu_sub.update(&imu);
if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation, perf_count(_imu_missed_perf);
PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
_vehicle_imu_sub.get_last_generation()); _vehicle_imu_sub.get_last_generation());
} }

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

@@ -176,6 +176,7 @@ private:
perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; 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 _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")};
// Used to check, save and use learned magnetometer biases // 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) hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)