|
/*! |
|
加速度センサーの出力をLCDに表示 |
|
* version V1.0 |
|
* date 2019-5-24 |
|
|
|
# LCD 16x02 module |
|
SDA --- LCD SDA |
|
SCL --- LCD SCL |
|
5V --- VCC |
|
GND --- GND |
|
|
|
# MPU-9250 module |
|
SDA --- A4 |
|
SCL --- A5 |
|
VCC --- 3.3V (LCDと異なるので注意!) |
|
GND --- GND |
|
|
|
SCL SDA VCC GND |
|
------------ |
|
| o o o o | |
|
| MPU-9250 | |
|
| BREAKOUT | |
|
| | |
|
| o o o o | |
|
------------ |
|
*/ |
|
|
|
// --- for mpu9250 |
|
#include "quaternionFilters.h" |
|
#include "MPU9250.h" |
|
// --- for lcd |
|
#include <Wire.h> |
|
#include "DFRobot_LCD.h" |
|
|
|
DFRobot_LCD lcd(16,2); //16 characters and 2 lines of show |
|
|
|
/* void breath(unsigned char color){ */ |
|
/* for(int i=0; i<255; i++){ */ |
|
/* lcd.setPWM(color, i); */ |
|
/* delay(5); */ |
|
/* } */ |
|
/* */ |
|
/* delay(500); */ |
|
/* for(int i=254; i>=0; i--){ */ |
|
/* lcd.setPWM(color, i); */ |
|
/* delay(5); */ |
|
/* } */ |
|
/* */ |
|
/* delay(500); */ |
|
/* } */ |
|
|
|
#define AHRS false // Set to false for basic data read |
|
#define SerialDebug true // Set to true to get Serial output for debugging |
|
#define I2Cclock 400000 |
|
#define I2Cport Wire |
|
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0 // Use either this line or the next to select which I2C address your device is using |
|
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1 |
|
MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock); |
|
|
|
void setup() { |
|
// initialize |
|
lcd.init(); |
|
|
|
// Read the WHO_AM_I register, this is a good test of communication |
|
// 定数MPU9250_ADDRESSとWHO_AM_I_MPU9250はlibraryに含まれている |
|
byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); |
|
Serial.print(F("MPU9250 I AM 0x")); |
|
Serial.print(c, HEX); |
|
Serial.print(F(" I should be 0x")); |
|
Serial.println(0x71, HEX); |
|
|
|
// Print a message to the LCD. |
|
lcd.setCursor(1, 0); lcd.print("DFRobot&MPU"); |
|
lcd.setCursor(1, 1); lcd.print("hello world"); |
|
delay(1000); |
|
|
|
//ifdef LCD |
|
lcd.setCursor(20,0); lcd.print("MPU9250"); |
|
lcd.setCursor(0,10); lcd.print("I AM"); |
|
lcd.setCursor(0,20); lcd.print(c, HEX); |
|
lcd.setCursor(0,30); lcd.print("I Should Be"); |
|
lcd.setCursor(0,40); lcd.print(0x71, HEX); |
|
lcd.display(); |
|
delay(1000); |
|
//endif // LCD |
|
|
|
|
|
if (c == 0x71) // WHO_AM_I should always be 0x71 |
|
{ |
|
Serial.println(F("MPU9250 is online...")); |
|
|
|
// Start by performing self test and reporting values |
|
myIMU.MPU9250SelfTest(myIMU.selfTest); |
|
Serial.print(F("x-axis self test: acceleration trim within : ")); |
|
Serial.print(myIMU.selfTest[0],1); Serial.println("% of factory value"); |
|
Serial.print(F("y-axis self test: acceleration trim within : ")); |
|
Serial.print(myIMU.selfTest[1],1); Serial.println("% of factory value"); |
|
Serial.print(F("z-axis self test: acceleration trim within : ")); |
|
Serial.print(myIMU.selfTest[2],1); Serial.println("% of factory value"); |
|
Serial.print(F("x-axis self test: gyration trim within : ")); |
|
Serial.print(myIMU.selfTest[3],1); Serial.println("% of factory value"); |
|
Serial.print(F("y-axis self test: gyration trim within : ")); |
|
Serial.print(myIMU.selfTest[4],1); Serial.println("% of factory value"); |
|
Serial.print(F("z-axis self test: gyration trim within : ")); |
|
Serial.print(myIMU.selfTest[5],1); Serial.println("% of factory value"); |
|
|
|
// Calibrate gyro and accelerometers, load biases in bias registers |
|
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias); |
|
|
|
lcd.clear(); |
|
|
|
lcd.setCursor(0, 0); lcd.print("MPU9250 bias"); |
|
lcd.setCursor(0, 8); lcd.print(" x y z "); |
|
|
|
lcd.setCursor(0, 16); lcd.print((int)(1000*myIMU.accelBias[0])); |
|
lcd.setCursor(24, 16); lcd.print((int)(1000*myIMU.accelBias[1])); |
|
lcd.setCursor(48, 16); lcd.print((int)(1000*myIMU.accelBias[2])); |
|
lcd.setCursor(72, 16); lcd.print("mg"); |
|
|
|
lcd.setCursor(0, 24); lcd.print(myIMU.gyroBias[0], 1); |
|
lcd.setCursor(24, 24); lcd.print(myIMU.gyroBias[1], 1); |
|
lcd.setCursor(48, 24); lcd.print(myIMU.gyroBias[2], 1); |
|
lcd.setCursor(66, 24); lcd.print("o/s"); |
|
|
|
lcd.display(); |
|
delay(1000); |
|
|
|
myIMU.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, WHO_AM_I_AK8963); |
|
Serial.print("AK8963 "); |
|
Serial.print("I AM 0x"); |
|
Serial.print(d, HEX); |
|
Serial.print(" I should be 0x"); |
|
Serial.println(0x48, HEX); |
|
|
|
|
|
lcd.clear(); |
|
lcd.setCursor(20,0); lcd.print("AK8963"); |
|
lcd.setCursor(0,10); lcd.print("I AM"); |
|
lcd.setCursor(0,20); lcd.print(d, HEX); |
|
lcd.setCursor(0,30); lcd.print("I Should Be"); |
|
lcd.setCursor(0,40); lcd.print(0x48, HEX); |
|
lcd.display(); |
|
delay(1000); |
|
|
|
|
|
if (d != 0x48) |
|
{ |
|
// Communication failed, stop here |
|
Serial.println(F("Communication failed, abort!")); |
|
Serial.flush(); |
|
abort(); |
|
} |
|
|
|
// Get magnetometer calibration from AK8963 ROM |
|
myIMU.initAK8963(myIMU.factoryMagCalibration); |
|
// 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 factory sensitivity adjustment value "); |
|
Serial.println(myIMU.factoryMagCalibration[0], 2); |
|
Serial.print("Y-Axis factory sensitivity adjustment value "); |
|
Serial.println(myIMU.factoryMagCalibration[1], 2); |
|
Serial.print("Z-Axis factory sensitivity adjustment value "); |
|
Serial.println(myIMU.factoryMagCalibration[2], 2); |
|
} |
|
|
|
|
|
lcd.clear(); |
|
lcd.setCursor(20,0); lcd.print("AK8963"); |
|
lcd.setCursor(0,10); lcd.print("ASAX "); |
|
lcd.setCursor(50,10); lcd.print(myIMU.factoryMagCalibration[0], 2); |
|
lcd.setCursor(0,20); lcd.print("ASAY "); |
|
lcd.setCursor(50,20); lcd.print(myIMU.factoryMagCalibration[1], 2); |
|
lcd.setCursor(0,30); lcd.print("ASAZ "); |
|
lcd.setCursor(50,30); lcd.print(myIMU.factoryMagCalibration[2], 2); |
|
lcd.display(); |
|
delay(1000); |
|
} // if (c == 0x71) |
|
else |
|
{ |
|
Serial.print("Could not connect to MPU9250: 0x"); |
|
Serial.println(c, HEX); |
|
|
|
// Communication failed, stop here |
|
Serial.println(F("Communication failed, abort!")); |
|
Serial.flush(); |
|
abort(); |
|
} |
|
} |
|
|
|
void loop() { |
|
/* breath(REG_ONLY); LCD 明滅 使わないだろ*/ |
|
// 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 |
|
|
|
// 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; // - myIMU.accelBias[0]; |
|
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1]; |
|
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2]; |
|
|
|
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values |
|
|
|
// Calculate the gyro value into actual degrees per second |
|
// This depends on scale being set |
|
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes; |
|
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes; |
|
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes; |
|
|
|
myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values |
|
|
|
// 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 = (float)myIMU.magCount[0] * myIMU.mRes |
|
* myIMU.factoryMagCalibration[0] - myIMU.magBias[0]; |
|
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes |
|
* myIMU.factoryMagCalibration[1] - myIMU.magBias[1]; |
|
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes |
|
* myIMU.factoryMagCalibration[2] - myIMU.magBias[2]; |
|
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) |
|
|
|
// Must be called before updating quaternions! |
|
myIMU.updateTime(); |
|
|
|
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of |
|
// the magnetometer; the magnetometer z-axis (+ down) 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 |
|
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); |
|
|
|
if (!AHRS) |
|
{ |
|
myIMU.delt_t = millis() - myIMU.count; |
|
if (myIMU.delt_t > 500) |
|
{ |
|
if(SerialDebug) |
|
{ |
|
// Print acceleration values in milligs! |
|
Serial.print("X-acceleration: "); Serial.print(1000 * myIMU.ax); |
|
Serial.print(" mg "); |
|
Serial.print("Y-acceleration: "); Serial.print(1000 * myIMU.ay); |
|
Serial.print(" mg "); |
|
Serial.print("Z-acceleration: "); Serial.print(1000 * myIMU.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("Y-gyro rate: "); Serial.print(myIMU.gy, 3); |
|
Serial.print(" degrees/sec "); |
|
Serial.print("Z-gyro rate: "); Serial.print(myIMU.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("Y-mag field: "); Serial.print(myIMU.my); |
|
Serial.print(" mG "); |
|
Serial.print("Z-mag field: "); Serial.print(myIMU.mz); |
|
Serial.println(" mG"); |
|
|
|
myIMU.tempCount = myIMU.readTempData(); // Read the adc values |
|
// Temperature in degrees Centigrade |
|
myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; |
|
// Print temperature in degrees Centigrade |
|
Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); |
|
Serial.println(" degrees C"); |
|
} |
|
|
|
#ifdef LCD |
|
lcd.clear(); |
|
lcd.setCursor(0, 0); lcd.print("MPU9250/AK8963"); |
|
lcd.setCursor(0, 8); lcd.print(" x y z "); |
|
|
|
lcd.setCursor(0, 16); lcd.print((int)(1000 * myIMU.ax)); |
|
lcd.setCursor(24, 16); lcd.print((int)(1000 * myIMU.ay)); |
|
lcd.setCursor(48, 16); lcd.print((int)(1000 * myIMU.az)); |
|
lcd.setCursor(72, 16); lcd.print("mg"); |
|
|
|
lcd.setCursor(0, 24); lcd.print((int)(myIMU.gx)); |
|
lcd.setCursor(24, 24); lcd.print((int)(myIMU.gy)); |
|
lcd.setCursor(48, 24); lcd.print((int)(myIMU.gz)); |
|
lcd.setCursor(66, 24); lcd.print("o/s"); |
|
|
|
lcd.setCursor(0, 32); lcd.print((int)(myIMU.mx)); |
|
lcd.setCursor(24, 32); lcd.print((int)(myIMU.my)); |
|
lcd.setCursor(48, 32); lcd.print((int)(myIMU.mz)); |
|
lcd.setCursor(72, 32); lcd.print("mG"); |
|
|
|
lcd.setCursor(0, 40); lcd.print("Gyro T "); |
|
lcd.setCursor(50, 40); lcd.print(myIMU.temperature, 1); |
|
lcd.print(" C"); |
|
lcd.display(); |
|
#endif // LCD |
|
|
|
myIMU.count = millis(); |
|
} // if (myIMU.delt_t > 500) |
|
} // if (!AHRS) |
|
else |
|
{ |
|
// Serial print and/or lcd at 0.5 s rate independent of data rates |
|
myIMU.delt_t = millis() - myIMU.count; |
|
|
|
// update LCD once per half-second independent of read rate |
|
if (myIMU.delt_t > 500) |
|
{ |
|
if(SerialDebug) |
|
{ |
|
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); |
|
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"); |
|
|
|
Serial.print("q0 = "); Serial.print(*getQ()); |
|
Serial.print(" qx = "); Serial.print(*(getQ() + 1)); |
|
Serial.print(" qy = "); Serial.print(*(getQ() + 2)); |
|
Serial.print(" qz = "); Serial.println(*(getQ() + 3)); |
|
} |
|
|
|
// 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. |
|
myIMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() |
|
* *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) |
|
* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) |
|
* *(getQ()+3)); |
|
myIMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() |
|
* *(getQ()+2))); |
|
myIMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) |
|
* *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) |
|
* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) |
|
* *(getQ()+3)); |
|
myIMU.pitch *= RAD_TO_DEG; |
|
myIMU.yaw *= RAD_TO_DEG; |
|
|
|
// Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is |
|
// 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 |
|
// - http://www.ngdc.noaa.gov/geomag-web/#declination |
|
myIMU.yaw -= 8.5; |
|
myIMU.roll *= RAD_TO_DEG; |
|
|
|
if(SerialDebug) |
|
{ |
|
Serial.print("Yaw, Pitch, Roll: "); |
|
Serial.print(myIMU.yaw, 2); |
|
Serial.print(", "); |
|
Serial.print(myIMU.pitch, 2); |
|
Serial.print(", "); |
|
Serial.println(myIMU.roll, 2); |
|
|
|
Serial.print("rate = "); |
|
Serial.print((float)myIMU.sumCount / myIMU.sum, 2); |
|
Serial.println(" Hz"); |
|
} |
|
|
|
#ifdef LCD |
|
lcd.clear(); |
|
|
|
lcd.setCursor(0, 0); lcd.print(" x y z "); |
|
|
|
lcd.setCursor(0, 8); lcd.print((int)(1000 * myIMU.ax)); |
|
lcd.setCursor(24, 8); lcd.print((int)(1000 * myIMU.ay)); |
|
lcd.setCursor(48, 8); lcd.print((int)(1000 * myIMU.az)); |
|
lcd.setCursor(72, 8); lcd.print("mg"); |
|
|
|
lcd.setCursor(0, 16); lcd.print((int)(myIMU.gx)); |
|
lcd.setCursor(24, 16); lcd.print((int)(myIMU.gy)); |
|
lcd.setCursor(48, 16); lcd.print((int)(myIMU.gz)); |
|
lcd.setCursor(66, 16); lcd.print("o/s"); |
|
|
|
lcd.setCursor(0, 24); lcd.print((int)(myIMU.mx)); |
|
lcd.setCursor(24, 24); lcd.print((int)(myIMU.my)); |
|
lcd.setCursor(48, 24); lcd.print((int)(myIMU.mz)); |
|
lcd.setCursor(72, 24); lcd.print("mG"); |
|
|
|
lcd.setCursor(0, 32); lcd.print((int)(myIMU.yaw)); |
|
lcd.setCursor(24, 32); lcd.print((int)(myIMU.pitch)); |
|
lcd.setCursor(48, 32); lcd.print((int)(myIMU.roll)); |
|
lcd.setCursor(66, 32); lcd.print("ypr"); |
|
|
|
// With these settings the filter is updating at a ~145 Hz rate using the |
|
// Madgwick scheme and >200 Hz using the Mahony scheme even though the |
|
// lcd refreshes at only 2 Hz. The filter update rate is determined |
|
// mostly by the mathematical steps in the respective algorithms, the |
|
// processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: |
|
// an ODR of 10 Hz for the magnetometer produce the above rates, maximum |
|
// magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and |
|
// ~38 Hz for the Madgwick and Mahony schemes, respectively. This is |
|
// presumably because the magnetometer read takes longer than the gyro or |
|
// accelerometer reads. This filter update rate should be fast enough to |
|
// maintain accurate platform orientation for stabilization control of a |
|
// fast-moving robot or quadcopter. Compare to the update rate of 200 Hz |
|
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 |
|
// 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty |
|
// well! |
|
lcd.setCursor(0, 40); lcd.print("rt: "); |
|
lcd.print((float) myIMU.sumCount / myIMU.sum, 2); |
|
lcd.print(" Hz"); |
|
lcd.display(); |
|
#endif // LCD |
|
|
|
myIMU.count = millis(); |
|
myIMU.sumCount = 0; |
|
myIMU.sum = 0; |
|
} // if (myIMU.delt_t > 500) |
|
} // if (AHRS) |
|
} |