uint16_t imu_gyro_z() { uint8_t receive_lower = imu_read(ZGYRO_OUT_LOWER); uint8_t receive_upper = imu_read(ZGYRO_OUT_UPPER); return (receive_lower + (receive_upper<<8)); }
int16_t imu_accl_x() { uint8_t receive_lower = imu_read(XACCL_OUT_LOWER); uint8_t receive_upper = imu_read(XACCL_OUT_UPPER); return ((receive_lower + (receive_upper<<8)) - X_ACCL_OFFSET); }
int16_t imu_gyro_x() { uint8_t receive_lower = imu_read(XGYRO_OUT_LOWER); uint8_t receive_upper = imu_read(XGYRO_OUT_UPPER); return ((receive_lower + (receive_upper<<8)) - X_GYRO_OFFSET); }
int16_t imu_accl_z() { uint8_t receive_lower = imu_read(ZACCL_OUT_LOWER); uint8_t receive_upper = imu_read(ZACCL_OUT_UPPER); return (receive_lower + (receive_upper<<8)); }
int Lua_ReadImuRaw(lua_State *L) { if (lua_gettop(L) >= 1) { unsigned int channel = (unsigned int)lua_tointeger(L,1); if (channel >= 0 && channel <= CONFIG_IMU_CHANNELS) { int value = imu_read(channel); lua_pushinteger(L,value); return 1; } } return 0; }
static void imu_flush_filter(size_t physicalChannel) { for (size_t i = 0; i < 1000; i++) { update_filter(&g_imu_filter[physicalChannel], imu_read(physicalChannel)); } }
void imu_sample_all() { for (size_t i = 0; i < CONFIG_IMU_CHANNELS; i++) { update_filter(&g_imu_filter[i], imu_read(i)); } }
// The loop function is called in an endless loop void loop() { //Add your repeated code here // if programming failed, don't try to do anything imu_read(); // if (!dmpReady) return; // // 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 // // . // // . // // . // } // // 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(F("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); // #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); // if(lastQ == q) { // Serial.println("*****************\nLOCKED\n************************"); // if(--count) { // dmpReady = false; // Serial.println("MPU stalled, reinitializing..."); // mpu.reset(); // if ((devStatus = mpu.dmpInitialize()) == 0) // { // mpu.setDMPEnabled(true); // mpuIntStatus = mpu.getIntStatus(); // dmpReady = true; // packetSize = mpu.dmpGetFIFOPacketSize(); // } // else { // Serial.print("DMP reinitialization failed (code "); // Serial.print(devStatus); // Serial.println(")"); // while (true) { // delay(300); // digitalWrite(LED_PIN, 0); // delay(300); // digitalWrite(LED_PIN, 1); // } // } // } // } // else { // // Serial.print("OK "); // count = 3; // lastQ = q; // mpu.dmpGetGravity(&gravity, &q); // mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // //Serial.print("ypr\t"); // Serial.print("DMP:"); // Serial.print(ypr[0] * 180/M_PI); // //Serial.print("\t"); // Serial.print(":"); // Serial.print(ypr[1] * 180/M_PI); // //Serial.print("\t"); // Serial.print(":"); // Serial.println(ypr[2] * 180/M_PI); // delay(1); // } // #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); // } }
// Get gyro Z uint16_t get_gyro_z(void) { uint8_t data[2]; imu_read(IMU_GYRO_ZOUT_H, data, 2); return ((uint16_t) data[0] << 8) | (data[1]); }
// Get accel Z uint16_t get_accel_z(void) { uint8_t data[2]; imu_read(IMU_ACCEL_ZOUT_H, data, 2); return ((uint16_t) data[0] << 8) | (data[1]); }