AP_InertialSensor_PX4: add optional debug

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

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__