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]);
}
