int ImuTester::run()
{
// Default is fail unless pass critera met
m_pass = TEST_FAIL;
// Register the driver
int ret = m_sensor.init();
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_PATH, h.getError());
m_done = true;
} else {
m_done = false;
}
while (!m_done) {
++m_read_attempts;
struct imu_sensor_data data;
ret = ImuSensor::getSensorData(h, data, true);
if (ret == 0) {
uint32_t count = data.read_counter;
DF_LOG_INFO("count: %d", count);
if (m_read_counter != count) {
m_read_counter = count;
ImuSensor::printImuValues(h, data);
}
} else {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
}
if (m_read_counter >= num_read_attempts) {
// Done test - PASSED
m_pass = TEST_PASS;
m_done = true;
} else if (m_read_attempts > num_read_attempts) {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
m_done = true;
}
}
DevMgr::releaseHandle(h);
DF_LOG_INFO("Closing IMU sensor");
m_sensor.stop();
return m_pass;
}
int main()
{
pc.baud(9600); // baud rate: 9600
mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
wait(1);
mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
pc.printf("Calibration is completed. \r\n");
wait(0.5);
mpu6050.init(); // Initialize the sensor
wait(1);
pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
wait_ms(500);
while(1)
{
/* Uncomment below if you want to see accel and gyro data */
// pc.printf(" _____________________________________________________________ \r\n");
// pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
// pc.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
// pc.printf("|_____________________________________________________________ \r\n\r\n");
//
// wait(2.5);
filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
pc.printf(" _______________\r\n");
pc.printf("| Pitch: %.3f \r\n",pitchAngle);
pc.printf("| Roll: %.3f \r\n",rollAngle);
pc.printf("|_______________\r\n\r\n");
wait(1);
}
}
