SECONDA PARTE
byte mpuIntStatus = SPIread(0x3A, ChipSelPin1); // by reading INT_STATUS register, all interrupts are cleared
Serial.println("done.");
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
Serial.println("DMP ready! Waiting for first data from MPU-6000...");
Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
}
else // have to check if this is still functional
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
} // end void setup()
// ################################ MAIN LOOP
// Sensor orientation ArduIMU+ V3:
// (all rotations "through" the MPU-6000 on the board)
// +X is ROLL rotation along longer dimension of the board (towards GPS connector - "front")
// +Y is PITCH rotation along shorter dimension of the board (towards GPS connector - "left")
// +Z is YAW rotation around line upwards (through the MPU-6000 - "top")
//
void loop()
{
// if DMP initialization during setup failed, don't try to do anything
if (!dmpReady)
{
Serial.println(">>> DMP FAIL");
return;
}
// wait for MPU interrupt or extra packet(s) available
// INFO: if there is an interrupt send from the MPU-6000 to the ATmega328P (the "Arduino" processor),
// boolean variable "mpuInterrupt" will be made "true" (see explanation in void setup() )
while ((mpuInterrupt == false) && (fifoCount < packetSize))
{
// do nothing until mpuInterrupt = true or fifoCount >= 42
}
// there has just been an interrupt, so reset the interrupt flag, then get INT_STATUS byte
mpuInterrupt = false;
byte mpuIntStatus = SPIread(0x3A, ChipSelPin1); // by reading INT_STATUS register, all interrupts are cleared
// get current FIFO count
fifoCount = getFIFOCount(ChipSelPin1);
// check for FIFO overflow (this should never happen unless our code is too inefficient or DEBUG output delays code too much)
if ((mpuIntStatus & 0x10) || fifoCount == 1008)
// mpuIntStatus & 0x10 checks register 0x3A for FIFO_OFLOW_INT
// the size of the FIFO buffer is 1024 bytes, but max. set to 1008 so after 24 packets of 42 bytes
// the FIFO is reset, otherwise the last FIFO reading before reaching 1024 contains only 16 bytes
// and can/will produces output value miscalculations
{
// reset so we can continue cleanly
//SPIwriteBit(0x6A, 6, false, ChipSelPin1); // FIFO_EN = 0 = disable
SPIwriteBit(0x6A, 2, true, ChipSelPin1); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0
//SPIwriteBit(0x6A, 6, true, ChipSelPin1); // FIFO_EN = 1 = enable
}
// otherwise, check for DMP data ready interrupt (this should happen frequently)
else if (mpuIntStatus & 0x02)
// mpuIntStatus & 0x02 checks register 0x3A for (undocumented) DMP_INT
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = getFIFOCount(ChipSelPin1);
// read a packet from FIFO
SPIreadBytes(0x74, packetSize, fifoBuffer, ChipSelPin1);
// track FIFO count here in case there is more than one packet (each of 42 bytes) available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount = fifoCount - packetSize;
// ============================================================================================== //
// >>>>>>>>> - from here the 42 FIFO bytes from the MPU-6000 can be used to generate output >>>>>>>>
// >>>>>>>>> - this would be the place to add your own code into >>>>>>>>
// >>>>>>>>> - of course all the normal MPU-6000 registers can be used here too >>>>>>>>
////////////////////////////////////////////////////////////////////////////////////////////////////
// ============================================================================================== //
// get the quaternion values from the FIFO - needed for Euler and roll/pitch/yaw angle calculations
int raw_q_w = ((fifoBuffer[0] << 8) + fifoBuffer[1]); // W
int raw_q_x = ((fifoBuffer[4] << 8) + fifoBuffer[5]); // X
int raw_q_y = ((fifoBuffer[8] << 8) + fifoBuffer[9]); // Y
int raw_q_z = ((fifoBuffer[12] << 8) + fifoBuffer[13]); // Z
float q_w = raw_q_w / 16384.0f;
float q_x = raw_q_x / 16384.0f;
float q_y = raw_q_y / 16384.0f;
float q_z = raw_q_z / 16384.0f;
#ifdef OUTPUT_READABLE_ROLLPITCHYAW
// display Euler angles in degrees
// dmpGetGravity
float grav_x = 2 * ((q_x * q_z) - (q_w * q_y));
float grav_y = 2 * ((q_w * q_x) + (q_y * q_z));
float grav_z = (q_w * q_w) - (q_x * q_x) - (q_y * q_y) + (q_z * q_z);
// roll: (tilt left/right, about X axis)
rpy_rol = atan(grav_y / (sqrt((grav_x * grav_x) + (grav_z * grav_z))));
// pitch: (nose up/down, about Y axis)
rpy_pit = atan(grav_x / (sqrt((grav_y * grav_y) + (grav_z * grav_z))));
// yaw: (rotate around Z axis)
rpy_yaw = atan2((2 * q_x * q_y) - (2 * q_w * q_z), (2 * q_w * q_w) + (2 * q_x * q_x) - 1);
x=rpy_pit * 180/M_PI;
Serial.print("Roll angle [degrees]: ");
// Serial.print(rpy_rol * 180/M_PI); Serial.print("\t");
Serial.print(x); Serial.print("\n");
// Serial.print(rpy_yaw * 180/M_PI); Serial.println();
//#ifdef OUTPUT_RAW_ANGLES
// print calculated angles for roll and pitch from the raw acceleration components
// (yaw is undetermined here, this needs the use of the quaternion - see further on)
int AcceX = ((fifoBuffer[28] << 8) + fifoBuffer[29]);
int AcceY = ((fifoBuffer[32] << 8) + fifoBuffer[33]);
int AcceZ = ((fifoBuffer[36] << 8) + fifoBuffer[37]);
// atan2 outputs the value of -pi to pi (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2 pi and then from radians to degrees - in the end it's 0 - 360 degrees
ADegX = (atan2(AcceY, AcceZ) + PI) * RAD_TO_DEG;
ADegY = (atan2(AcceX, AcceZ) + PI) * RAD_TO_DEG;
#endif
}}
DA QUI IN POI TROVIAMO:
-SPI read/write functions
-DMP functions used in dmpInitialize()
-Main DMP initialize function
che non ho riportato per l'eccessiva mole di codice (1200 righe). Ciò che ho postato dovrebbe essere sufficiente a capire dove crasha il programma.
STAMPA le inizializzazioni ma non parte il loop con i dati rilevati!
############# MPU-6000 Data Acquisition #############
Initializing SPI Protocol...
...SPI Protocol initializing done.
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Initializing Digital Motion Processor (DMP)...
Writing DMP memory.......... done.
Verifying DMP memory.......... success!
Writing DMP configuration... done.
Verifying DMP configuration... success!
Writing DMP update 1/7 ..... done.
Verifying DMP update 1/7 ..... success!
Writing DMP update 2/7 ..... done.
Verifying DMP update 2/7 ..... success!
Writing DMP update 3/7 ..... done.
Verifying DMP update 3/7 ..... success!
Writing DMP update 4/7 ..... done.
Verifying DMP update 4/7 ..... success!
Writing DMP update 5/7 ..... done.
Verifying DMP update 5/7 ..... success!
Writing DMP update 6/7 ..... done.
Verifying DMP update 6/7 ..... success!
Writing DMP update 7/7 ..... done.
Verifying DMP update 7/7 ..... success!
... Digital Motion Processor (DMP) initializing done.
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Enabling DMP... done.
Enabling interrupt detection... done.
DMP ready! Waiting for first data from MPU-6000...
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Qualche consiglio?