void loop() { while(1){ mpuInterrupt = false; // char buf[10]={0}; // // UART_TX((uint8_t *)buf, sprintf(buf,"%u\r\n", HAL_GetTick())); // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { // cnt++; // time_now = HAL_GetTick(); // if((time_now-time_old) >= 1000){ // time_old = time_now; // char buffer[10]={0}; // UART_TX((uint8_t *)buffer, sprintf(buffer,"%u\r\n", (int)cnt )); // UART_TX((uint8_t*)"\r\n", sizeof("\r\n")); // cnt=0; // } // UART_TX((uint8_t*)"x=", sizeof("x=")); // UART_Float_TX( (ypr[2] * 180/M_PI) ); // // UART_TX((uint8_t*)"y=", sizeof("y=")); // UART_Float_TX( (ypr[1] * 180/M_PI) ); // // UART_TX((uint8_t*)"z=", sizeof("z=")); // UART_Float_TX( (ypr[0] * 180/M_PI) ); // UART_TX((uint8_t*)"\n\r", sizeof("\n\r")); // BSP_LED_Toggle(LED4); // UART_TX((uint8_t*)"MAIN\r\n", sizeof("MAIN\r\n")); // other program behavior stuff here // . // . // . // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . // . // . } // reset interrupt flag and get INT_STATUS byte mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus == 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); UART_TX((uint8_t*)"FIFO Overflow\r\n", sizeof("FIFO Overflow\r\n")); BSP_LED_Toggle(LED5); } // otherwise, check for DMP data ready interrupt (this should happen frequently) else if(mpuIntStatus == 0x01) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print("quat\t"); Serial.print(q.w); Serial.print("\t"); Serial.print(q.x); Serial.print("\t"); Serial.print(q.y); Serial.print("\t"); Serial.println(q.z); #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print("euler\t"); Serial.print(euler[0] * 180/M_PI); Serial.print("\t"); Serial.print(euler[1] * 180/M_PI); Serial.print("\t"); Serial.println(euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // trace_printf("ypr\t"); // trace_printf("%f", ypr[0] * 180/M_PI); // trace_printf("\t"); // trace_printf("%d", ypr[1] * 180/M_PI); // trace_printf("\t"); // trace_printf("%d\n", ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print("areal\t"); Serial.print(aaReal.x); Serial.print("\t"); Serial.print(aaReal.y); Serial.print("\t"); Serial.println(aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif // blink LED to indicate activity // BSP_LED_Toggle(LED3); } } }
void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; Serial.println("fireeer!"); // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { // other program behavior stuff here // . // . // . // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . // . // . // harry: try to reset here Serial.println("hang? reset!"); mpu.reset(); } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); Serial.println("FIFO overflow!"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print("quat\t"); Serial.print(q.w); Serial.print("\t"); Serial.print(q.x); Serial.print("\t"); Serial.print(q.y);; Serial.print("\t"); Serial.println(q.z); quaternionW = q.w; #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print("euler\t"); Serial.print(euler[0] * 180/M_PI); Serial.print("\t"); Serial.print(euler[1] * 180/M_PI); Serial.print("\t"); Serial.println(euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); Serial.print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print("areal\t"); Serial.print(aaReal.x); Serial.print("\t"); Serial.print(aaReal.y); Serial.print("\t"); Serial.println(aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }
void *thread_sensor(void* arg) { //setup(); /* mpu6050.Init(); measurement.setTo(Scalar(0)); kalman.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); kalman.statePre.at<float>(0) = mpu6050.accelX(); kalman.statePre.at<float>(1) = mpu6050.accelY(); kalman.statePre.at<float>(2) = mpu6050.accelZ(); kalman.statePre.at<float>(3) = 0.0; setIdentity(kalman.measurementMatrix); setIdentity(kalman.processNoiseCov, Scalar::all(1e-4)); setIdentity(kalman.measurementNoiseCov, Scalar::all(10)); setIdentity(kalman.errorCovPost, Scalar::all(.1)); */ //double x, y, z; //double xSpeed = 0, ySpeed = 0, zSpeed = 0; //double X = 0, Y = 0, Z = 0; Quaternion q; // [w, x, y, z] quaternion container while (1) { kalman.predict(); /* x = mpu6050.accelX(); y = mpu6050.accelY(); z = mpu6050.accelZ(); measurement(0) = x; measurement(1) = y; measurement(2) = z; */ readFIFO(); // Calcurate Gravity and Yaw, Pitch, Roll mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&accelRaw, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&accelReal, &accelRaw, &gravity); mpu.dmpGetLinearAccelInWorld(&accelWorld, &accelReal, &q); measurement(0) = accelWorld.x; measurement(1) = accelWorld.y; measurement(2) = accelWorld.z; estimated = kalman.correct(measurement); /* xSpeed += estimated.at<float>(0) * INTERVAL / 1000; // speed m/s ySpeed += estimated.at<float>(1) * INTERVAL / 1000; zSpeed += estimated.at<float>(2) * INTERVAL / 1000; X += xSpeed * INTERVAL / 1000; // speed * INTERVAL / 1000 * 1000 displacement in mm Y += ySpeed * INTERVAL / 1000; Z += zSpeed * INTERVAL / 1000; Quaternion q(0.0, x, y, z); VectorFloat vector[3]; GetGravity(vector, &q); float ypr[3]; GetYawPitchRoll(ypr, &q, vector); //mpu6050.Next(); */ printf("%8.5f, %8.5f, %8.5f\n", estimated.at<float>(0), estimated.at<float>(1), estimated.at<float>(2)); usleep(1000 * INTERVAL); } return NULL; }
void loop() { // if programming failed, don't try to do anything if (!b_dmp_ready) return; // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && uh_fifo_count < uh_packet_size) { } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; ua_mpu_interrupt_status = mpu.getIntStatus(); // get current FIFO count uh_fifo_count = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((ua_mpu_interrupt_status & 0x10) || uh_fifo_count == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); send_status(FIFO_OVERFLOW, ua_mpu_interrupt_status); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (ua_mpu_interrupt_status & 0x02) { uint8_t ua_idx, ua_nb = 0; uint8_t ua_data_len = 1; uint8_t ua_types = 0; uint8_t *pua_buf, *pua_data_buf, *pua_data_start; // wait for correct available data length, should be a VERY short wait while (uh_fifo_count < uh_packet_size) { uh_fifo_count = mpu.getFIFOCount(); } // read a packet from FIFO mpu.getFIFOBytes(ua_fifo_buffer, uh_packet_size); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) uh_fifo_count -= uh_packet_size; #ifdef OUTPUT_BUFFER ua_data_len += BUFFER_SIZE; ua_types |= OUTPUT_BUFFER; #else // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&s_quaternion, ua_fifo_buffer); #endif #ifdef OUTPUT_QUATERNION ua_data_len += 4 * sizeof(float); ua_types |= OUTPUT_QUATERNION; #endif #ifdef OUTPUT_EULER mpu.dmpGetEuler(rf_euler, &s_quaternion); ua_data_len += 3 * sizeof(float); ua_types |= OUTPUT_EULER; #endif #if defined(OUTPUT_YAWPITCHROLL) || defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL) mpu.dmpGetGravity(&s_gravity, &s_quaternion); #endif #ifdef OUTPUT_YAWPITCHROLL mpu.dmpGetYawPitchRoll(rf_ypr, &s_quaternion, &s_gravity); ua_data_len += 3 * sizeof(float); ua_types |= OUTPUT_YAWPITCHROLL; #endif #if defined(OUTPUT_REALACCEL) || defined(OUTPUT_WORLDACCEL) // display real acceleration, adjusted to remove gravity mpu.dmpGetAccel(&s_acceleration, ua_fifo_buffer); mpu.dmpGetLinearAccel(&s_acceleration_real, &s_acceleration, &s_gravity); #endif #ifdef OUTPUT_REALACCEL ua_data_len += 3 * sizeof(float); ua_types |= OUTPUT_REALACCEL; #endif #ifdef OUTPUT_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity mpu.dmpGetLinearAccelInWorld(&s_acceleration_world, &s_acceleration_real, &s_quaternion); ua_data_len += 3 * sizeof(float); ua_types |= OUTPUT_WORLDACCEL; #endif // allocate the buffe to store the values pua_data_start = (uint8_t*)malloc(ua_data_len); // Store the start of the buffer pua_data_buf = pua_data_start; // Setup type of data expected *pua_data_buf++ = ua_types; #ifdef OUTPUT_BUFFER *pua_data_buf++ = ua_fifo_buffer[0]; *pua_data_buf++ = ua_fifo_buffer[1]; *pua_data_buf++ = ua_fifo_buffer[4]; *pua_data_buf++ = ua_fifo_buffer[5]; *pua_data_buf++ = ua_fifo_buffer[8]; *pua_data_buf++ = ua_fifo_buffer[9]; *pua_data_buf++ = ua_fifo_buffer[12]; *pua_data_buf++ = ua_fifo_buffer[13]; #endif #ifdef OUTPUT_QUATERNION pua_data_buf += store_float32(pua_data_buf, s_quaternion.w); pua_data_buf += store_float32(pua_data_buf, s_quaternion.x); pua_data_buf += store_float32(pua_data_buf, s_quaternion.y); pua_data_buf += store_float32(pua_data_buf, s_quaternion.z); #endif #ifdef OUTPUT_EULER pua_data_buf += store_float32(pua_data_buf, rf_euler[0] * 180/M_PI); pua_data_buf += store_float32(pua_data_buf, rf_euler[1] * 180/M_PI); pua_data_buf += store_float32(pua_data_buf, rf_euler[2] * 180/M_PI); #endif #ifdef OUTPUT_YAWPITCHROLL pua_data_buf += store_float32(pua_data_buf, rf_ypr[0] * 180/M_PI); pua_data_buf += store_float32(pua_data_buf, rf_ypr[1] * 180/M_PI); pua_data_buf += store_float32(pua_data_buf, rf_ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_REALACCEL pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.x); pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.y); pua_data_buf += store_float32(pua_data_buf, s_acceleration_real.z); #endif #ifdef OUTPUT_WORLDACCEL pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.x); pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.y); pua_data_buf += store_float32(pua_data_buf, s_acceleration_world.z); #endif // send the result pua_buf = frame_get((uint8_t*)pua_data_start, ua_data_len); if (pua_buf != NULL) { Serial.write(pua_buf, FULL_SIZE + ua_data_len); free(pua_buf); } free(pua_data_start); // blink LED to indicate activity b_blink_state = !b_blink_state; digitalWrite(LED_PIN, b_blink_state); } }
void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; // get current FIFO count fifoCount = mpu.getFIFOCount(); if (fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); printf("FIFO overflow!\n"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (fifoCount >= 42) { // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); printf("quat %7.2f %7.2f %7.2f %7.2f ", q.w,q.x,q.y,q.z); #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); printf("euler %7.2f %7.2f %7.2f ", euler[0] * 180/M_PI, euler[1] * 180/M_PI, euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); printf("ypr %7.2f %7.2f %7.2f ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); printf("areal %6d %6d %6d ", aaReal.x, aaReal.y, aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif printf("\n"); } }