diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index 613d9e184d..fc7c94f339 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -49,6 +49,7 @@ AK09916::AK09916(const I2CSPIDriverConfig &config) : AK09916::~AK09916() { + perf_free(_reset_perf); perf_free(_transfer_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); @@ -64,6 +65,8 @@ int AK09916::init() return ret; } + _retries = 2; + return Reset() ? 0 : -1; } @@ -79,6 +82,7 @@ void AK09916::print_status() { I2CSPIDriverBase::print_status(); + perf_print_counter(_reset_perf); perf_print_counter(_transfer_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); @@ -108,6 +112,7 @@ void AK09916::RunImpl() { switch (_state) { case STATE::RESET: + perf_count(_reset_perf); // CNTL3 SRST: Soft reset RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); _reset_timestamp = hrt_absolute_time(); @@ -179,6 +184,8 @@ void AK09916::RunImpl() const int16_t z = combine(buffer.HZH, buffer.HZL); // sensor's frame is +X forward (X), +Y right (Y), +Z down (Z) + _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_magnetic_sensor_overflow_perf)); _px4_mag.update(timestamp_sample, x, y, z); success = true; diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 98ad0dcb9a..3ab00c092b 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -97,6 +97,7 @@ private: PX4Magnetometer _px4_mag; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};