aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor_PX4: add optional debug

Browse Source
This commit is contained in:
Jonathan Challinger 2015-02-20 00:34:14 -08:00 committed by Andrew Tridgell
parent 074ee49cd0
commit bc655ff0cc
2 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -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()

11
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -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__