#include "quaternionfilters.h" #include "MPU9250.h" data read #define SerialDebug true // Set to true to get Serial output for debugging

Transcription

1 /*Hardware setup: MPU9250 Breakout Arduino VDD V VDDI V SDA A4 SCL A5 GND GND */ #include "quaternionfilters.h" #include "MPU9250.h" #define AHRS true Set to false for basic data read #define SerialDebug true Set to true to get Serial output for debugging beginning SD slot stuff

2 /* SD card datalogger This example shows how to log data from three analog sensors to an SD card using the SD library. The circuit: * analog sensors on analog ins 0, 1, and 2 * SD card attached to SPI bus as follows: ** MOSI - pin 11 ** MISO - pin 12 ** CLK - pin 13 ** CS - pin 4 (for MKRZero SD: SDCARD_SS_PIN) */ #include <SPI.h> #include <SD.h> const int chipselect = 4; void setup() Open serial communications and wait for port to open: Serial.begin(9600); while (!Serial) ; wait for serial port to connect. Needed for native USB port only }

3 Serial.print("Initializing SD card..."); see if the card is present and can be initialized: if (!SD.begin(chipSelect)) Serial.println("Card failed, or not present"); don't do anything more: return; } Serial.println("card initialized."); } void loop() make a string for assembling the data to log: String datastring = ""; read three sensors and append to the string: for (int analogpin = 0; analogpin < 3; analogpin++) int sensor = analogread(analogpin); datastring += String(sensor); if (analogpin < 2) datastring += ","; } } open the file. note that only one file can be open at a time, so you have to close this one before opening another.

4 File datafile = SD.open("datalog.txt", FILE_WRITE); if the file is available, write to it: if (datafile) datafile.println(datastring); datafile.close(); print to the serial port too: Serial.println(dataString); } if the file isn't open, pop up an error: else Serial.println("error opening datalog.txt"); } } end of SD slot stuff Pin definitions int intpin = 12; These can be changed, 2 and 3 are the Arduinos ext int pins int myled = 13; Set up pin 13 led for toggling MPU9250 myimu; MPU9250 myimu1;

5 void setup() TWBR = 12; 400 kbit/sec I2C speed Serial.begin(9600); Wire.begin(); Set up the interrupt pin, its set as active high, push-pull pinmode(intpin, INPUT); digitalwrite(intpin, LOW); pinmode(myled, OUTPUT); digitalwrite(myled, HIGH); Read the WHO_AM_I register, this is a good test of communication byte c = myimu.readbyte(0x68, WHO_AM_I_MPU9250); byte e = myimu1.readbyte(0x69, WHO_AM_I_MPU9250); Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX); Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(e, HEX); Serial.print(" I should be "); Serial.println(0xFF, HEX); if ((c == 0x71)&&(e == 0xFF)) WHO_AM_I should

6 always be 0x68, OR (c == 0x71)&&(e == 0xFF) Serial.println("Both MPU9250's are online..."); Start by performing self test and reporting values myimu.mpu9250selftest(myimu.selftest); myimu1.mpu9250selftest(myimu1.selftest); Calibrate gyro and accelerometers, load biases in bias registers myimu.calibratempu9250(myimu.gyrobias, myimu. accelbias); myimu1.calibratempu9250(myimu1.gyrobias, myimu1. accelbias); myimu.initmpu9250(); myimu1.initmpu9250(); Initialize device for active mode read of acclerometer, gyroscope, and temperature Serial.println("MPU9250 initialized for active data mode..."); Read the WHO_AM_I register of the magnetometer, this is a good test of communication byte d = myimu.readbyte(ak8963_address,

7 WHO_AM_I_AK8963); Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX); possible redefinition of object Get magnetometer calibration from AK8963 ROM myimu.initak8963(myimu.magcalibration); myimu1.initak8963(myimu1.magcalibration); Initialize device for active mode read of magnetometer Serial.println("AK8963 initialized for active data mode..."); if (SerialDebug) Serial.println("Calibration values: "); Serial.print("X-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[0], 2); Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[1], 2); Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[2], 2); Serial.print("0x69: X-Axis sensitivity adjustment value "); Serial.println(myIMU1.magCalibration[0], 2);

8 Serial.print("0x69: Y-Axis sensitivity adjustment value "); Serial.println(myIMU1.magCalibration[1], 2); Serial.print("0x69: Z-Axis sensitivity adjustment value "); Serial.println(myIMU1.magCalibration[2], 2); } } if (c == 0x71) else Serial.print("Could not connect to MPU9250: 0x"); Serial.println(c, HEX); while(1) ; Loop forever if communication doesn't happen } } void loop() If intpin goes high, all data registers have new data On interrupt, check if data ready interrupt if (myimu.readbyte(mpu9250_address, INT_STATUS) & 0x01) myimu.readacceldata(myimu.accelcount); Read the x/y/z adc values myimu1.readacceldata(myimu1.accelcount);

9 myimu.getares(); myimu1.getares(); Now we'll calculate the accleration value into actual g's This depends on scale being set myimu.ax = (float)myimu.accelcount[0]*myimu.ares; - accelbias[0]; myimu1.ax = (float)myimu1.accelcount[0]*myimu1. ares; - accelbias[0]; myimu.ay = (float)myimu.accelcount[1]*myimu.ares; - accelbias[1]; myimu1.ay = (float)myimu1.accelcount[1]*myimu1. ares; - accelbias[1]; myimu.az = (float)myimu.accelcount[2]*myimu.ares; - accelbias[2]; myimu1.az = (float)myimu1.accelcount[2]*myimu1. ares; - accelbias[2]; myimu.readgyrodata(myimu.gyrocount); Read the x/y/z adc values myimu1.readgyrodata(myimu1.gyrocount); Read the x/y/z adc values myimu.getgres(); myimu1.getgres(); Calculate the gyro value into actual degrees per second This depends on scale being set myimu.gx = (float)myimu.gyrocount[0]*myimu.gres; myimu1.gx = (float)myimu1.gyrocount[0]*myimu1.gres;

10 myimu.gy = (float)myimu.gyrocount[1]*myimu.gres; myimu1.gy = (float)myimu1.gyrocount[1]*myimu1.gres; myimu.gz = (float)myimu.gyrocount[2]*myimu.gres; myimu1.gz = (float)myimu1.gyrocount[2]*myimu1.gres; myimu.readmagdata(myimu.magcount); Read the x/y/z adc values myimu1.readmagdata(myimu1.magcount); Read the x/y/z adc values myimu.getmres(); myimu1.getmres(); User environmental x-axis correction in milligauss, should be automatically calculated myimu.magbias[0] = +470.; myimu1.magbias[0] = +470.; User environmental x-axis correction in milligauss TODO axis?? myimu.magbias[1] = +120.; myimu1.magbias[1] = +120.; User environmental x-axis correction in milligauss myimu.magbias[2] = +125.; myimu1.magbias[2] = +125.; Calculate the magnetometer values in milligauss Include factory calibration per data sheet and user environmental corrections Get actual magnetometer value, this depends on scale being set myimu.mx =

11 (float)myimu.magcount[0]*myimu.mres*myimu. magcalibration[0] - myimu.magbias[0]; myimu1.mx = (float)myimu1.magcount[0]*myimu1.mres*myimu1. magcalibration[0] - myimu1.magbias[0]; myimu.my = (float)myimu.magcount[1]*myimu.mres*myimu. magcalibration[1] - myimu.magbias[1]; myimu1.my = (float)myimu1.magcount[1]*myimu1.mres*myimu1. magcalibration[1] - myimu1.magbias[1]; myimu.mz = (float)myimu.magcount[2]*myimu.mres*myimu. magcalibration[2] - myimu.magbias[2]; myimu1.mz = (float)myimu1.magcount[2]*myimu1.mres*myimu1. magcalibration[2] - myimu1.magbias[2]; } if (readbyte(mpu9250_address, INT_STATUS) & 0x01) Must be called before updating quaternions! myimu.updatetime(); myimu1.updatetime(); Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer; the magnetometer z-axis (+ down)

12 is opposite to z-axis (+ up) of accelerometer and gyro! We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter. For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. This is ok by aircraft orientation standards! Pass gyro rate as rad/s MadgwickQuaternionUpdate(ax, ay, az, gx*pi/180.0f, gy*pi/180.0f, gz*pi/180.0f, my, mx, mz); MahonyQuaternionUpdate(myIMU.ax, myimu.ay, myimu.az, myimu.gx*deg_to_rad, myimu.gy*deg_to_rad, myimu. gz*deg_to_rad, myimu.my, myimu.mx, myimu.mz, myimu. deltat); MahonyQuaternionUpdate(myIMU1.ax, myimu1.ay, myimu1. az, myimu1.gx*deg_to_rad, myimu1.gy*deg_to_rad, myimu1. gz*deg_to_rad, myimu1.my, myimu1.mx, myimu1.mz, myimu1. deltat); if (!AHRS) myimu.delt_t = millis() - myimu.count;

13 myimu1.delt_t = millis() - myimu1.count; if ((myimu.delt_t > 500) && (myimu1.delt_t > 500)) if(serialdebug) Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000*myIMU.ax); Serial.print(" mg "); Serial.print("MPU1: X-acceleration: "); Serial.print(1000*myIMU1.ax); Serial.print(" mg "); Serial.print("Y-acceleration: "); Serial.print(1000*myIMU.ay); Serial.print(" mg "); Serial.print("MPU1: Y-acceleration: "); Serial.print(1000*myIMU1.ay); Serial.print(" mg "); Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az); Serial.println(" mg "); Serial.print("MPU1: Z-acceleration: "); Serial.print(1000*myIMU1.az); Serial.println(" mg "); Print gyro values in degree/sec Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3); Serial.print(" degrees/sec "); Serial.print("MPU1: X-gyro rate: ");

14 Serial.print(myIMU1.gx, 3); Serial.print(" degrees/sec "); Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3); Serial.print(" degrees/sec "); Serial.print("MPU1: Y-gyro rate: "); Serial.print(myIMU1.gy, 3); Serial.print(" degrees/sec "); Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3); Serial.println(" degrees/sec"); Serial.print("MPU1: Z-gyro rate: "); Serial.print(myIMU1.gz, 3); Serial.println(" degrees/sec"); Print mag values in degree/sec Serial.print("X-mag field: "); Serial.print(myIMU.mx); Serial.print(" mg "); Serial.print("MPU1: X-mag field: "); Serial.print(myIMU1.mx); Serial.print(" mg "); Serial.print("Y-mag field: "); Serial.print(myIMU.my); Serial.print(" mg "); Serial.print("MPU1: Y-mag field: "); Serial.print(myIMU1.my); Serial.print(" mg "); Serial.print("Z-mag field: "); Serial.print(myIMU.mz); Serial.println(" mg");

15 Serial.print("MPU1: Z-mag field: "); Serial.print(myIMU.mz); Serial.println(" mg"); myimu.tempcount = myimu.readtempdata(); myimu1.tempcount = myimu1.readtempdata(); Read the adc values Temperature in degrees Centigrade myimu.temperature = ((float) myimu.tempcount) / ; myimu1.temperature = ((float) myimu1.tempcount) / ; Print temperature in degrees Centigrade Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); Serial.println(" degrees C"); Serial.print("MPU1: Temperature is "); Serial.print(myIMU1.temperature, 1); Serial.println(" degrees C"); } myimu.count = millis(); myimu1.count = millis(); digitalwrite(myled,!digitalread(myled)); toggle led } if (myimu.delt_t > 500) } if (!AHRS) else Serial print and/or display at 0.5 s rate

16 independent of data rates myimu.delt_t = millis() - myimu.count; myimu1.delt_t = millis() - myimu1.count; int arrayvalue = 0; float dataarray[arrayvalue]; ADDING THE TITLES & UNITS FOR 2 SENSORS char titlestr1[19] = 'ms ', '1-ax ', '1-ay ', '1-az ', '1-gx ', '1-gy ', '1-gz ', '1-mx ', '1-my ', '1-mz ', '2-ax ', '2-ay ', '2-az ', '2-gx ', '2-gy ', '2-gz ', '2-mx ', '2-my ', '2-mz '}; Serial.println(titleStr1[19]); update LCD once per 100ms independent of read rate if (myimu.delt_t > 100 && myimu1.delt_t > 100) if(serialdebug) UNCOMMENT THESE IF SIMPLE PRINTING IS DESIRED a,g,m for 0x68 Serial.print("ax = "); Serial.print((int)1000*myIMU.ax); Serial.print(" ay = "); Serial.print((int)1000*myIMU.ay); Serial.print(" az = "); Serial.print((int)1000*myIMU.az);

17 Serial.println(" mg"); Serial.print("gx = "); Serial.print( myimu. gx, 2); Serial.print(" gy = "); Serial.print( myimu. gy, 2); Serial.print(" gz = "); Serial.print( myimu. gz, 2); Serial.println(" deg/s"); Serial.print("mx = "); Serial.print( (int)myimu.mx ); Serial.print(" my = "); Serial.print( (int)myimu.my ); Serial.print(" mz = "); Serial.print( (int)myimu.mz ); Serial.println(" mg"); a,g,m for 0x69 Serial.print("MPU2: ax = "); Serial.print((int)1000*myIMU1.ax); Serial.print(" ay = "); Serial.print((int)1000*myIMU1.ay); Serial.print(" az = "); Serial.print((int)1000*myIMU1.az); Serial.println(" mg"); Serial.print("MPU2: gx = "); Serial.print(

18 myimu1.gx, 2); Serial.print(" gy = "); Serial.print( myimu1. gy, 2); Serial.print(" gz = "); Serial.print( myimu1. gz, 2); Serial.println(" deg/s"); Serial.print("MPU2: mx = "); Serial.print( (int)myimu1.mx ); Serial.print(" my = "); Serial.print( (int)myimu1.my ); Serial.print(" mz = "); Serial.print( (int)myimu1.mz ); Serial.println(" mg"); THIS PRINTS THE DATA INTO AN ARRAY WITH THE FIRST VALUE THE TIMESTAMP AND THEN THE VALUES FOR A,G, and M for IMUs a - 0x68 dataarray[arrayvalue] = myimu.count}; dataarray[arrayvalue] = (int)1000*myimu.ax};

19 dataarray[arrayvalue] = (int)1000*myimu.ay}; dataarray[arrayvalue] = (int)1000*myimu.az}; g's - 0x68 dataarray[arrayvalue] = myimu.gx}; dataarray[arrayvalue] = myimu.gy}; dataarray[arrayvalue] = myimu.gz}; m's - 0x68 dataarray[arrayvalue] = (int)myimu.mx};

20 dataarray[arrayvalue] = (int)myimu.my}; dataarray[arrayvalue] = (int)myimu.mz}; a's - 0x69 dataarray[arrayvalue] = (int)1000*myimu1.ax}; dataarray[arrayvalue] = (int)1000*myimu1.ay}; dataarray[arrayvalue] = (int)1000*myimu1.az}; g's - 0x69 dataarray[arrayvalue] = myimu1.gx};

21 dataarray[arrayvalue] = myimu1.gy}; dataarray[arrayvalue] = myimu1.gz}; m's - 0x69 dataarray[arrayvalue] = (int)myimu1.mx}; dataarray[arrayvalue] = (int)myimu1.my}; dataarray[arrayvalue] = (int)myimu1.mz}; Serial.println(" "); } myimu.count = millis(); myimu1.count = millis();

22 myimu.sumcount = 0; myimu1.sumcount = 0; myimu.sum = 0; myimu1.sum = 0; } if (myimu.delt_t > 500) } if (AHRS) }