دوك imu تءارق للاخ نم هنم دكاتلاو ليدعتلا لازم يبي لدعم scoop
دوك imu تءارق للاخ نم هنم دكاتلاو ليدعتلا لازم يبي لدعم scoop
/*
MPU9
250
Basi
c
Exam
ple
Code
SDA and SCL should have external pull-up resistors (to 3.3V).
10k resistors are on the EMSENSR-9250 breakout board.
Hardware setup:
MPU9250 Breakout --------- Arduino
VDD ---------------------- 3.3V
VDDI --------------------- 3.3V
SDA ----------------------- A4
SCL ----------------------- A5
GND ---------------------- GND
Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a
3.3 V Teensy 3.1.
We have disabled the internal pull-ups used by the Wire library in the
Wire.h/twi.c utility file.
We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L
/twi.h utility file.
*/
#include <SPI.h>
#include <Wire.h>
//#include <Adafruit_GFX.h>
//#include <Adafruit_PCD8544.h>
// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-
00, Rev. 1.4, 9/9/2013 for registers not listed in
// above document; the MPU9250 and MPU9150 are virtually identical but the
latter has a different register map
//
//Magnetometer Registers
#define AK8963_ADDRESS 0x0C
#define WHO_AM_I_AK8963 0x00 // should return 0x48
#define INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status
bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001),
self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};
enum Mscale {
MFS_14BITS = 0, // 0.6 mG per LSB
MFS_16BITS // 0.15 mG per LSB
};
// 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
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference
System)
float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in
rads/s (start at 40 deg/s)
float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in
rad/s/s (start at 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response
speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError
of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a
stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not
fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response
time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the
I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges,
usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion
scheme.
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other
free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and
fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f
uint32_t delt_t = 0; // used to control display output rate
uint32_t count = 0, sumCount = 0; // used to control display output rate
float pitch, yaw, roll;
float deltat = 0.0f, sum = 0.0f; // integration interval for both filter
schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration
interval
uint32_t Now = 0; // used to calculate integration interval
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor
data values
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for
Mahony method
float magBias[3],magScale[3];
void setup()
{
Wire.begin();
// TWBR = 12; // 400 kbit/sec I2C speed
Serial.begin(115200);
Serial.println("MPU9250 bias");
Serial.println(" x y z ");
Serial.print((int)(1000*accelBias[0]));
Serial.print((int)(1000*accelBias[1]));
Serial.print((int)(1000*accelBias[2]));
Serial.print("mg");
Serial.print(gyroBias[0], 1);
Serial.print(gyroBias[1], 1);
Serial.print(gyroBias[2], 1);
Serial.println("o/s");
delay(1000);
initMPU9250();
Serial.println("MPU9250 initialized for active data mode...."); //
Initialize device for active mode read of acclerometer, gyroscope, and
temperature
delay(1000);
if(SerialDebug) {
// Serial.println("Calibration values: ");
Serial.print("X-Axis sensitivity adjustment value ");
Serial.println(magCalibration[0], 2);
Serial.print("Y-Axis sensitivity adjustment value ");
Serial.println(magCalibration[1], 2);
Serial.print("Z-Axis sensitivity adjustment value ");
Serial.println(magCalibration[2], 2);
}
// display.clearDisplay();
// display.setCursor(20,0);
Serial.println("AK8963");
// display.setCursor(0,10);
Serial.println("ASAX ");
// display.setCursor(50,10);
Serial.println(magCalibration[0], 2);
// display.setCursor(0,20);
Serial.println("ASAY ");
// display.setCursor(50,20);
Serial.println(magCalibration[1], 2);
// display.setCursor(0,30);
Serial.println("ASAZ ");
// display.setCursor(50,30);
Serial.println(magCalibration[2], 2);
// display.display();
delay(1000);
}
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
if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if
data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
getAres();
Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time
elapsed since last filter update
lastUpdate = Now;
if (!AHRS) {
delt_t = millis() - count;
if(delt_t > 500) {
if(SerialDebug) {
// Print acceleration values in milligs!
// Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg
");
// Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg
");
// Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println("
mg ");
//
// // Print gyro values in degree/sec
// Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print("
degrees/sec ");
// Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print("
degrees/sec ");
// Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println("
degrees/sec");
//
// // Print mag values in degree/sec
// Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG ");
// Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG ");
// Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG");
// display.clearDisplay();
// display.setCursor(0, 0); Serial.println("MPU9250/AK8963");
// display.setCursor(0, 8); Serial.println(" x y z ");
//
// display.setCursor(0, 16); Serial.println((int)(1000*ax));
// display.setCursor(24, 16); Serial.println((int)(1000*ay));
// display.setCursor(48, 16); Serial.println((int)(1000*az));
// display.setCursor(72, 16); Serial.println("mg");
//
// display.setCursor(0, 24); Serial.println((int)(gx));
// display.setCursor(24, 24); Serial.println((int)(gy));
// display.setCursor(48, 24); Serial.println((int)(gz));
// display.setCursor(66, 24); Serial.println("o/s");
//
// display.setCursor(0, 32); Serial.println((int)(mx));
// display.setCursor(24, 32); Serial.println((int)(my));
// display.setCursor(48, 32); Serial.println((int)(mz));
// display.setCursor(72, 32); Serial.println("mG");
//
// display.setCursor(0, 40); Serial.println("Gyro T ");
// display.setCursor(50, 40); Serial.println(temperature, 1);
Serial.println(" C");
// display.display();
//
count = millis();
digitalWrite(myLed, !digitalRead(myLed)); // toggle led
}
}
else {
if(SerialDebug) {
// Serial.print("ax = "); Serial.print((int)1000*ax);
// Serial.print(" ay = "); Serial.print((int)1000*ay);
// Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
// Serial.print("gx = "); Serial.print( gx, 2);
// Serial.print(" gy = "); Serial.print( gy, 2);
// Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
// Serial.print("mx = "); Serial.print( (int)mx );
// Serial.print(" my = "); Serial.print( (int)my );
// Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
//
// Serial.print("q0 = "); Serial.print(q[0]);
// Serial.print(" qx = "); Serial.print(q[1]);
// Serial.print(" qy = "); Serial.print(q[2]);
// Serial.print(" qz = "); Serial.println(q[3]);
}
// if(SerialDebug) {
// Serial.print("Yaw, Pitch, Roll: ");
// Serial.print(yaw, 2);
// Serial.print(", ");
// Serial.print(pitch, 2);
// Serial.print(", ");
// Serial.println(roll, 2);
//
//// Serial.print("rate = "); Serial.print((float)sumCount/sum, 2);
Serial.println(" Hz");
// }
// display.clearDisplay();
//
// display.setCursor(0, 0); Serial.println(" x y z ");
//
// display.setCursor(0, 8); Serial.println((int)(1000*ax));
// display.setCursor(24, 8); Serial.println((int)(1000*ay));
// display.setCursor(48, 8); Serial.println((int)(1000*az));
// display.setCursor(72, 8); Serial.println("mg");
//
// display.setCursor(0, 16); Serial.println((int)(gx));
// display.setCursor(24, 16); Serial.println((int)(gy));
// display.setCursor(48, 16); Serial.println((int)(gz));
// display.setCursor(66, 16); Serial.println("o/s");
//
// display.setCursor(0, 24); Serial.println((int)(mx));
// display.setCursor(24, 24); Serial.println((int)(my));
// display.setCursor(48, 24); Serial.println((int)(mz));
// display.setCursor(72, 24); Serial.println("mG");
//
// display.setCursor(0, 32); Serial.println((int)(yaw));
// display.setCursor(24, 32); Serial.println((int)(pitch));
// display.setCursor(48, 32); Serial.println((int)(roll));
// display.setCursor(66, 32); Serial.println("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 display 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!
// display.setCursor(0, 40);
// Serial.println("rt: "); Serial.println((float) sumCount / sum, 2);
Serial.println(" Hz");
// display.display();
count = millis();
sumCount = 0;
sum = 0;
}
}
//==============================================================================
=====================================
//====== Set of useful function to access acceleration. gyroscope, magnetometer,
and temperature data
//==============================================================================
=====================================
void getMres() {
switch (Mscale)
{
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
mRes = 10.*4912./8190.; // Proper scale to return milliGauss
break;
case MFS_16BITS:
mRes = 10.*4912./32760.0; // Proper scale to return milliGauss
break;
}
}
void getGres() {
switch (Gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
2-bit value:
case GFS_250DPS:
gRes = 250.0/32768.0;
break;
case GFS_500DPS:
gRes = 500.0/32768.0;
break;
case GFS_1000DPS:
gRes = 1000.0/32768.0;
break;
case GFS_2000DPS:
gRes = 2000.0/32768.0;
break;
}
}
void getAres() {
switch (Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
2-bit value:
case AFS_2G:
aRes = 2.0/32768.0;
break;
case AFS_4G:
aRes = 4.0/32768.0;
break;
case AFS_8G:
aRes = 8.0/32768.0;
break;
case AFS_16G:
aRes = 16.0/32768.0;
break;
}
}
int16_t readTempData()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw
data registers sequentially into data array
return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into
a 16-bit value
}
void initMPU9250()
{
// wake up device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6),
enable all sensors
delay(100); // Wait for all registers to reset
// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
// but all these rates are further reduced by a factor of 5 to 200 Hz because
of the SMPLRT_DIV setting
// reset device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset
bit; toggle reset device
delay(100);
// get stable time source; Auto select clock source to be PLL gyroscope
reference if ready
// else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);
}
accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average
count biases
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
// Construct the gyro biases for push to the hardware gyro bias registers, which
are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per
deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change
sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) // Should return percent deviation
from factory trim values, +/- 14 or less deviation is a pass
{
uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
uint8_t selfTest[6];
int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
float factoryTrim[6];
uint8_t FS = 0;
for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and
acclerometer
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the
six raw data registers into data array
aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the
MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as
average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro
and acclerometer
for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as
average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of
the Self-Test Response
// To get percent, must multiply by 100
for (int i = 0; i < 3; i++) {
destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i];
// Report percent differences
destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3];
// Report percent differences
}
imu but not Arduino mega وهذا مزال يبي التعديل وهذا ختي هوا
/*
Advanced_I2C.ino
Brian R Taylor
[email protected]
Copyright (c) 2017 Bolder Flight Systems
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without
restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software
is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or
substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
void loop() {
// read the sensor
IMU.readSensor();