 #include <i2c_t3.h>
 #include <SPI.h>
 #include <TinyGPS++.h>
+#include <RF22.h>
 
 #include <Data_module.h>
 #include <Sensor_helper.h>


  // begin I2C for MPU IMU
  Wire1.begin(I2C_MASTER, 0x000, I2C_PINS_29_30, I2C_PULLUP_EXT, I2C_RATE_400);
  
  // begin serial used for GPS data
  gpsSerial.begin(gpsSerialBaud);

  // Sensor setup
  delay(500);
  setupIMU();
  setupGyro();

  dataModule.initComplete();
}

void loop() {
  readGPS();

  readIMU();
  readGyro();

  delay(200);
  dataModule.println(getIMULogString(accelData)+getIMULogString(gyroData)+getIMULogString(magData));
  dataModule.println(getGyroLogString(L3G4200DData));
  dataModule.println("Got " + String(gps.charsProcessed(), DEC) + " chars");
}

// ------------------------------------------------------------


// function to be called each iteration to feed the GPS instantiation
void readGPS() {
  while (gpsSerial.available()) {
    gps.encode(gpsSerial.read());
  }
}

// Blocking function to read info from ESP
void readESP() {
  while (espSerial.available()) {
    dataModule.print(espSerial.read());
  }
}

