void gyro_acc() { // 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_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 t:%d ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI,count_time); count_time++; #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 }
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* gyro_acc(void*) { //float kp = 0.00375,ki = 0.0000,kd = 0.00076; float kp = 0.0068,ki = 0.000,kd = 0.0018; //0030 0088 0014 有偏角 p0.0031偏角更大 0.0029也是 i=0 小偏角 p0.00305 d0.00143 不错 i0.0005 偏角变大 //0032 0017 float pregyro =0; float desired = 0; //double error; float integ=0;//integral积分参数 float iLimit =8 ; float deriv=0;//derivative微分参数 float prevError=0; float lastoutput=0; //float Piddeadband=0.3; // initialize device printf("Initializing I2C devices...\n"); mpu.initialize(); // verify connection printf("Testing device connections...\n"); printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n"); mpu.setI2CMasterModeEnabled(false); mpu.setI2CBypassEnabled(true); // load and configure the DMP printf("Initializing DMP...\n"); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready printf("Enabling DMP...\n"); mpu.setDMPEnabled(true); // enable Arduino interrupt detection //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); //attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it printf("DMP ready!\n"); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) printf("DMP Initialization failed (code %d)\n", devStatus); } /*****************************************************/ while(1) { if (START_FLAG == 0) { delay(200); } if (START_FLAG == 1) { break; } } delay(50); for(;;) { if (!dmpReady) return 0; // 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); // 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); Angle[2] = ypr[0] * 180/M_PI; Angle[1] = ypr[1] * 180/M_PI;//此为Pitch Angle[0] = ypr[2] * 180/M_PI;//此为Roll // 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); AngleSpeed[0] = aaWorld.x; AngleSpeed[1] = aaWorld.y; AngleSpeed[2] = aaWorld.z; /****************************读取完毕*********************************/ error = desired - Angle[0];//偏差:期望-测量值 All_Count = All_Count + 1; error = error * 0.88 + prevError * 0.12; /* if (fabs(prevError - error ) > 12) { error = prevError; }*/ integ += error * IMU_UPDATE_DT;//偏差积分,IMU_UPDATE_DT也就是每调整漏斗大小的步辐 if (integ >= iLimit)//作积分限制 { integ = iLimit; } else if (integ < -iLimit) { integ = -iLimit; } deriv = (error - prevError) / IMU_UPDATE_DT;//微分 应该可用陀螺仪角速度代替 AngleSpeed[0] = deriv; if (fabs(deriv) < 20 ) { if (fabs(deriv) < 10 ) { deriv = deriv * 0.8; } else { deriv = deriv * 0.9; } } //if(deriv //roll.deriv = -gyro;//注意是否跟自己的参数方向相反,不然会加剧振荡 //deriv = -AngleSpeed[0]; /* if (fabs(pregyro - deriv) > 20) { deriv = deriv * 0.5 + pregyro * 0.5; } */ output = (kp * error) + (ki * integ) + (kd * deriv); prevError = error;//更新前一次偏差 pregyro = deriv; if (output > 0.16) { output = 0.16; } if (output < -0.16) { output = -0.16; } Pid_Roll = output; //output = output * 0.9 + lastoutput * 0.1; if (fabs(error) < 0.3 ) { output = lastoutput * 0.5; } lastoutput = output; DutyCycle[0] = Default_Acc - output; DutyCycle[1] = Default_Acc - output; //DutyCycle[0] = Default_Acc; //DutyCycle[1] = Default_Acc; DutyCycle[2] = Default_Acc + output; DutyCycle[3] = Default_Acc + output; //DutyCycle[2] = Default_Acc; //DutyCycle[3] = Default_Acc; PWMOut(PinNumber1,DutyCycle[0]); PWMOut(PinNumber2,DutyCycle[1]); PWMOut(PinNumber3,DutyCycle[2]); PWMOut(PinNumber4,DutyCycle[3]); } } }
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); } }
static void setup() { // initialize device printf("Initializing I2C devices...\n"); mpu.initialize(); // verify connection printf("Testing device connections...\n"); printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n"); // load and configure the DMP printf("Initializing DMP...\n"); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready printf("Enabling DMP...\n"); mpu.setDMPEnabled(true); // enable Arduino interrupt detection //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); //attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it printf("DMP ready! Waiting for first interrupt...\n"); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) printf("DMP Initialization failed (code %d)\n", devStatus); } /* adjAccel[0] = adjAccel[1] = adjAccel[2] = 0; adjGyro[0] = adjGyro[1] = adjGyro[2] = 0; for (int i = 0; i < 20; i++) { readFIFO(); mpu.dmpGetAccel(accel, fifoBuffer); mpu.dmpGetGyro(gyro, fifoBuffer); adjAccel[0] += accel[0]; adjAccel[1] += accel[1]; adjAccel[2] += accel[2]; adjGyro[0] += gyro[0]; adjGyro[1] += gyro[1]; adjGyro[2] += gyro[2]; } adjAccel[0] /= 20; adjAccel[1] /= 20; adjAccel[2] /= 20; adjGyro[0] /= 20; adjGyro[1] /= 20; adjGyro[2] /= 20; printf("ADJUST: %d, %d, %d\n", adjAccel[0], adjAccel[1], adjAccel[2]); */ measurement.setTo(cv::Scalar(0)); kalman.transitionMatrix = *(cv::Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); readFIFO(); mpu.dmpGetAccel(accel, fifoBuffer); kalman.statePre.at<float>(0) = accel[0]; kalman.statePre.at<float>(1) = accel[1]; kalman.statePre.at<float>(2) = accel[2]; kalman.statePre.at<float>(3) = 0.0; setIdentity(kalman.measurementMatrix); setIdentity(kalman.processNoiseCov, cv::Scalar::all(1e-4)); setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(10)); setIdentity(kalman.errorCovPost, cv::Scalar::all(.1)); }
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 (!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"); } }