From bc655ff0cc3df0e9645ee9761329abdf0937947d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 20 Feb 2015 00:34:14 -0800 Subject: [PATCH] AP_InertialSensor_PX4: add optional debug --- .../AP_InertialSensor_PX4.cpp | 31 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor_PX4.h | 11 +++++++ 2 files changed, 42 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index e762ad1bbb..89c859f067 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -243,6 +243,22 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep // report error count _set_accel_error_count(frontend_instance, accel_report.error_count); + +#ifdef AP_INERTIALSENSOR_PX4_DEBUG + _accel_dt_max[i] = max(_accel_dt_max[i],dt); + + _accel_meas_count[i] ++; + + if(_accel_meas_count[i] >= 10000) { + uint32_t tnow = hal.scheduler->micros(); + + ::printf("a%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0/((tnow-_accel_meas_count_start_us[i])*1.0e-6f),_accel_dt_max[i]); + + _accel_meas_count_start_us[i] = tnow; + _accel_meas_count[i] = 0; + _accel_dt_max[i] = 0; + } +#endif // AP_INERTIALSENSOR_PX4_DEBUG } void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report) @@ -285,6 +301,21 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report // report error count _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count); +#ifdef AP_INERTIALSENSOR_PX4_DEBUG + _gyro_dt_max[i] = max(_gyro_dt_max[i],dt); + + _gyro_meas_count[i] ++; + + if(_gyro_meas_count[i] >= 10000) { + uint32_t tnow = hal.scheduler->micros(); + + ::printf("g%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0f/((tnow-_gyro_meas_count_start_us[i])*1.0e-6f), _gyro_dt_max[i]); + + _gyro_meas_count_start_us[i] = tnow; + _gyro_meas_count[i] = 0; + _gyro_dt_max[i] = 0; + } +#endif // AP_INERTIALSENSOR_PX4_DEBUG } void AP_InertialSensor_PX4::_get_sample() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index c07b39b7e2..3dac2c1dc8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -75,6 +75,17 @@ private: Vector3f _delta_velocity_accumulator[INS_MAX_INSTANCES]; Vector3f _last_delAng[INS_MAX_INSTANCES]; Vector3f _last_gyro[INS_MAX_INSTANCES]; + +#ifdef AP_INERTIALSENSOR_PX4_DEBUG + uint32_t _gyro_meas_count[INS_MAX_INSTANCES]; + uint32_t _accel_meas_count[INS_MAX_INSTANCES]; + + uint32_t _gyro_meas_count_start_us[INS_MAX_INSTANCES]; + uint32_t _accel_meas_count_start_us[INS_MAX_INSTANCES]; + + float _gyro_dt_max[INS_MAX_INSTANCES]; + float _accel_dt_max[INS_MAX_INSTANCES]; +#endif // AP_INERTIALSENSOR_PX4_DEBUG }; #endif // CONFIG_HAL_BOARD #endif // __AP_INERTIAL_SENSOR_PX4_H__