FreeIMU::FreeIMU() {
#if HAS_ADXL345()
acc = ADXL345();
#elif HAS_BMA180()
acc = BMA180();
#endif
#if HAS_HMC5883L()
magn = HMC58X3();
#endif
#if HAS_ITG3200()
gyro = ITG3200();
#elif HAS_MPU6050()
accgyro = MPU60X0(); // I2C
#elif HAS_MPU6000()
accgyro = MPU60X0(); // SPI for Arduimu v3
#endif
#if HAS_MS5611()
baro = MS561101BA();
#endif
// initialize quaternion
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;
twoKp = twoKpDef;
twoKi = twoKiDef;
integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
lastUpdate = 0;
now = 0;
#ifndef CALIBRATION_H
// initialize scale factors to neutral values
acc_scale_x = 1;
acc_scale_y = 1;
acc_scale_z = 1;
magn_scale_x = 1;
magn_scale_y = 1;
magn_scale_z = 1;
#else
// get values from global variables of same name defined in calibration.h
acc_off_x = ::acc_off_x;
acc_off_y = ::acc_off_y;
acc_off_z = ::acc_off_z;
acc_scale_x = ::acc_scale_x;
acc_scale_y = ::acc_scale_y;
acc_scale_z = ::acc_scale_z;
magn_off_x = ::magn_off_x;
magn_off_y = ::magn_off_y;
magn_off_z = ::magn_off_z;
magn_scale_x = ::magn_scale_x;
magn_scale_y = ::magn_scale_y;
magn_scale_z = ::magn_scale_z;
#endif
}
/*
* Setup
*/
void setup() {
wdt_enable(WDTO_8S);
wdt_reset();
//Setup Ports
Serial.begin(115200); //Start Debug Serial 0
Serial1.begin(9600); //Start GPS Serial 1
Serial2.begin(9600);
pinMode(PIN_LED_GREEN, OUTPUT); //Blue GREEN
pinMode(PIN_LED_RED, OUTPUT); //Blue RED
pinMode(PIN_LED_BLUE, OUTPUT); //Blue LED
pinMode(PIN_SPI_CS,OUTPUT); //Chip Select Pin for the SD Card
pinMode(10, OUTPUT); //SDcard library expect 10 to set set as output.
// Initialise the GPS
wdt_disable();
gps.init();
gps.configureUbloxSettings(); // Configure Ublox for MY_HIGH altitude mode
wdt_enable(WDTO_8S);
// join I2C bus //start I2C transfer to the Module/Transmitter
Wire.begin();
//Set up the two EasyTransfer methods
ETI2Cout.begin(details(mD.i2cOut), &Wire); //setup the data structure to transfer out
ETSerialIn.begin(details(vals), &Serial2);
//Start up the LGgyro
if (LGgyro.init()) {
#ifdef DEBUG_ON
Serial.println("LGgyro OK");
#endif
LGgyro.enableDefault();
} else {
#ifdef DEBUG_ON
Serial.println("LGgyro not working");
#endif
SET_LED_Status(SET_LED_WHITE,500); //White LED
SET_LED_Status(SET_LED_RED,1000); //Red LED
}
//Start up the accelerometer
accel = ADXL345(); // Create an instance of the accelerometer
if(accel.EnsureConnected()) { // Check that the accelerometer is connected.
#ifdef DEBUG_ON
Serial.println("Connected to ADXL345.");
#endif
accel.SetRange(2, true); // Set the range of the accelerometer to a maximum of 2G.
accel.EnableMeasurements(); // Tell the accelerometer to start taking measurements.
} else{
#ifdef DEBUG_ON
Serial.println("Could not connect to ADXL345.");
#endif
SET_LED_Status(SET_LED_WHITE,500); //White LED
SET_LED_Status(SET_LED_RED,2000); //Red LED
}
//Start up the compass
compass = HMC5883L(); // Construct a new HMC5883 compass.
#ifdef DEBUG_ON
if(compass.EnsureConnected() == 1) {
Serial.println("Connected to HMC5883L.");
} else {
Serial.println("Not Connected to HMC5883L.");
}
#endif
error = compass.SetScale(1.3); // Set the scale of the compass.
#ifdef DEBUG_ON
if(error != 0) { // If there is an error, print it out.
Serial.println("Compass Error 1");
Serial.println(compass.GetErrorText(error));
} else {
Serial.println("Compass Ok 1");
}
#endif
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
#ifdef DEBUG_ON
if(error != 0) { // If there is an error, print it out.
Serial.println("Compass error 2");
Serial.println(compass.GetErrorText(error));
} else {
Serial.println("Compass Ok 2");
}
#endif
//Start up the Pressure Sensor
dps = BMP085();
dps.init();
#ifdef DEBUG_ON
Serial.print("BMP Mode ");
Serial.println(dps.getMode());
#endif
wdt_reset();
// Start up the OneWire Sensors library and turn off blocking takes too long!
sensors.begin();
sensors.setWaitForConversion(false);
sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperature
//Initialise all of the record values
mD.vals.tCount = 0;
mD.vals.uslCount = 0;
mD.vals.year = 0;
mD.vals.month = 0;
mD.vals.day = 0;
mD.vals.hour = 0;
mD.vals.minute = 0;
mD.vals.second = 0;
mD.vals.hundredths = 0;
mD.vals.iLat = 0;
mD.vals.iLong = 0;
mD.vals.iAlt = 0;
mD.vals.bSats = 0;
mD.vals.iAngle = 0;
mD.vals.iHspeed = 0;
mD.vals.iVspeed = 0;
mD.vals.age = 0;
mD.vals.ihdop = 0;
mD.vals.AcXPayload = 0;
mD.vals.AcYPayload = 0;
mD.vals.AcZPayload = 0;
mD.vals.GyXPayload = 0;
mD.vals.GyYPayload = 0;
mD.vals.GyZPayload = 0;
mD.vals.MgXPayload = 0;
mD.vals.MgYPayload = 0;
mD.vals.MgZPayload = 0;
mD.vals.TmpPayload = 0;
//Connect to the SD Card
if(!SD.begin(PIN_SPI_CS, SPI_HALF_SPEED)) {
#ifdef DEBUG_ON
Serial.println("SD not working!!");
#endif
SET_LED_Status(SET_LED_WHITE,500); //White LED
SET_LED_Status(SET_LED_RED,3000); //Red LED
} else {
#ifdef DEBUG_ON
Serial.println("SD OK");
#endif
dataFile.open(SD_LOG_FILE, O_CREAT | O_WRITE | O_APPEND); //Open Logfile
if (!dataFile.isOpen()) {
#ifdef DEBUG_ON
Serial.println("SD Data File Not Opened");
#endif
SET_LED_Status(SET_LED_WHITE,500);
SET_LED_Status(SET_LED_RED,3000);
}
}
//Cycle lights
SET_LED_Status(SET_LED_OFF,0);
SET_LED_Status(SET_LED_RED,500);
SET_LED_Status(SET_LED_GREEN,500);
SET_LED_Status(SET_LED_BLUE,500);
SET_LED_Status(SET_LED_OFF,0);
elapseSIM900 = millis(); //Elapse counter for data to SIM900
elapseNTXB = millis(); //Elapse counter for data to NTXB
NEWGPSDATA = false;
wdt_enable(WDTO_2S);
wdt_reset();
}