void
MPU9250::cycle_trampoline(void *arg)
{
MPU9250 *dev = (MPU9250 *)arg;
dev->cycle();
}
void
MPU9250::measure_trampoline(void *arg)
{
MPU9250 *dev = reinterpret_cast<MPU9250 *>(arg);
/* make another measurement */
dev->measure();
}
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()
{
//-------------------------------------------------------------------------
MPU9250 imu;
imu.initialize();
/* test code
if(imu.testConnection()){
printf("true");
}
else{
printf("false");
}*/
float ax, ay, az, gx, gy, gz, mx, my, mz, pitch, roll, realX, realY, heading, cx, cy, cz;
// FILE * pFile;
// pFile = fopen("AGMOUTPUT.txt","w");
//-------------------------------------------------------------------------
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
//WEIRDASS AXES - SEE DATASHEET
// cx = gy;
// cy = gx;
// cz = -gz;
//adjust for tilt
//pitch = cy * (PI / 180);
//roll = cx * (PI / 180);
//realX = cx*cos(pitch) + cz*sin(pitch);
//realY = cx*sin(roll)*sin(pitch) + cy*cos(roll) - cz*sin(roll)*sin(pitch);
//Calculate heading
//heading = (180 / PI) * atan2(realY, realX);
//printf("Heading: %+07.3f", heading);
/*
// print to file
// fprintf(pFile,"Heading: %+07.3f\n", heading);
// fprintf(pFile,"Acc: %+07.3f %+07.3f %+07.3f ", ax, ay, az);
fprintf(pFile,"Gyr: %+07.3f %+07.3f %+07.3f", gx, gy, gz);
fprintf(pFile,"Mag: %+07.3f %+07.3f %+07.3f\n", mx, my, mz);
fflush(pFile);
*/
//Nice Pretty Terminal output/
printf("Acc: %+07.3f %07.3f %+07.3f ", ax, ay, az);
printf("Gyr: %+07.3f %+07.3f %+07.3f ", gx, gy, gz);
printf("Mag: %+07.3f %+07.3f %+07.3f\n", mx, my, mz);
fflush(stdout);
}
int main(int argc, char *argv[])
{
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
ros::init(argc, argv, "ros_erle_imu_euler");
ros::NodeHandle n;
ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);
ros::Rate loop_rate(50);
while (ros::ok()){
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0)
usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&roll, &pitch, &yaw);
ros_erle_imu_euler::euler msg;
msg.roll = roll;
msg.pitch = pitch;
msg.yaw = yaw;
imu_euler_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void imuSetup()
{
//----------------------- MPU initialization ------------------------------
imu.initialize();
//-------------------------------------------------------------------------
printf("Beginning Gyro calibration...\n");
for(int i = 0; i<100; i++)
{
imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
offset[0] += (-gx*0.0175);
offset[1] += (-gy*0.0175);
offset[2] += (-gz*0.0175);
usleep(10000);
}
offset[0]/=100.0;
offset[1]/=100.0;
offset[2]/=100.0;
printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
int main()
{
pc.baud(38400);
//Set up I2C
i2c.frequency(400000); // use fast (400 kHz) I2C
//pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
t.start();
// Read the WHO_AM_I register, this is a good test of communication
uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
//pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
if (whoami == 0x71) // WHO_AM_I should always be 0x68
{
//pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
//pc.printf("MPU9250 is online...\n\r");
wait(1);
mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
/*pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
*/
mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
/*
pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
pc.printf("x accel bias = %f\n\r", accelBias[0]);
pc.printf("y accel bias = %f\n\r", accelBias[1]);
pc.printf("z accel bias = %f\n\r", accelBias[2]);
*/
wait(2);
mpu9250.initMPU9250();
//pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
mpu9250.initAK8963(magCalibration);
/*pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); */
wait(1);
}
else
{
pc.printf("Could not connect to MPU9250: \n\r");
pc.printf("%#x \n", whoami);
while(1) ; // Loop forever if communication doesn't happen
}
mpu9250.getAres(); // Get accelerometer sensitivity
mpu9250.getGres(); // Get gyro sensitivity
mpu9250.getMres(); // Get magnetometer sensitivity
/*pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); */
magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
magbias[1] = +120.; // User environmental x-axis correction in milliGauss
magbias[2] = +125.; // User environmental x-axis correction in milliGauss
myled2=!myled2;
while(1) {
myled2=!myled2;
myled=!myled;
// If intPin goes high, all data registers have new data
if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float)accelCount[1]*aRes - accelBias[1];
az = (float)accelCount[2]*aRes - accelBias[2];
mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
gy = (float)gyroCount[1]*gRes - gyroBias[1];
gz = (float)gyroCount[2]*gRes - gyroBias[2];
mpu9250.readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
}
Now = t.read_us();
deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat;
sumCount++;
// if(lastUpdate - firstUpdate > 10000000.0f) {
// beta = 0.04; // decrease filter gain after stabilized
// zeta = 0.015; // increasey bias drift gain after stabilized
// }
// Pass gyro rate as rad/s
// mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = t.read_ms() - count;
// if (delt_t > 500) { // update LCD once per half-second independent of read rate
/*
pc.printf("ax = %f", 1000*ax);
pc.printf(" ay = %f", 1000*ay);
pc.printf(" az = %f mg\n\r", 1000*az);
pc.printf("gx = %f", gx);
pc.printf(" gy = %f", gy);
pc.printf(" gz = %f deg/s\n\r", gz);
pc.printf("gx = %f", mx);
pc.printf(" gy = %f", my);
pc.printf(" gz = %f mG\n\r", mz);
*/
tempCount = mpu9250.readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
/*
pc.printf(" temperature = %f C\n\r", temperature);
pc.printf("q0 = %f\n\r", q[0]);
pc.printf("q1 = %f\n\r", q[1]);
pc.printf("q2 = %f\n\r", q[2]);
pc.printf("q3 = %f\n\r", q[3]);
*/
/* lcd.clear();
lcd.printString("MPU9250", 0, 0);
lcd.printString("x y z", 0, 1);
sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
lcd.printString(buffer, 0, 2);
sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
lcd.printString(buffer, 0, 3);
sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
lcd.printString(buffer, 0, 4);
*/
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll *= 180.0f / PI;
/*
pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
pc.printf("average rate = %f\n\r", (float) sumCount/sum); */
// sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
// lcd.printString(buffer, 0, 4);
// sprintf(buffer, "rate = %f", (float) sumCount/sum);
// lcd.printString(buffer, 0, 5);
pc.printf("%f %f %f\n\r",gx,gy,gz);
count = t.read_ms();
if(count > 1<<21) {
t.start(); // start the timer over again if ~30 minutes has passed
count = 0;
deltat= 0;
lastUpdate = t.read_us();
myled= !myled;
myled2=!myled2;
// }
sum = 0;
sumCount = 0;
}
}
}
