double Sensor::getBMP085altitude() {
BMP085 dps = BMP085();
Wire.begin();
dps.init();
long Temperature = 0, Pressure = 0, Altitude = 0;
dps.getTemperature(&Temperature);
dps.getPressure(&Pressure);
dps.getAltitude(&Altitude);
return (double) Altitude;
}
double Sensor::getBMP085pressure() {
Serial.println("At getPressure");
BMP085 dps = BMP085();
Wire.begin();
dps.init();
long Temperature = 0, Pressure = 0, Altitude = 0;
dps.getTemperature(&Temperature);
dps.getPressure(&Pressure);
dps.getAltitude(&Altitude);
Serial.print("The pressure is: ");Serial.println("Pressure");
return (double) Pressure;
}
int main(int argc, char **argv)
{
const char *fileName = "/dev/i2c-1";
int countdown = 0;
int blink = 1;
float timeStep = 83.33;
int printTerminal = 1;
int fd;// File description
string buttonInput;
float lastBlink = 0.0f;
bool isOnLED = false;
float timeStamp = 0.0f;
int delay = 0;
CTimeClass timer;
Logger logger;
ADXL345 acc;
BMP085 baro;
L3G4200D gyro;
HMC5883L compass;
GPIOClass gpioLED("17");
GPIOClass gpioButton("4");
// init GPIOs
gpioLED.export_gpio();
gpioButton.export_gpio();
gpioLED.setdir_gpio("out"); //GPIO4 set to output
gpioButton.setdir_gpio("in"); // GPIO17 set to input
// switch off LED just in case
gpioLED.setval_gpio("0");
// parse input arguments
if( argc > 1 ) {
parseArgv( countdown, blink, timeStep, printTerminal, argc, argv );
printf("c = %d, b = %d, t = %f, p = %d\n", countdown, blink, timeStep, printTerminal);
}
// init i2c
if(!init(&fd, fileName)) {
exit(1);
}
// wait for a little
usleep(100000);
// init imu
acc.init(&fd, ((int) ((1000/timeStep)+0.5)), timeStep);
baro.init(&fd);
compass.init(&fd);
gyro.init(&fd, ((int) ((1000/timeStep)+0.5)));
if( countdown > 0 ) {
waitUntilButton(gpioButton, gpioLED, countdown, blink );
}
timer.init();
if( logger.openLogFile( timeStep, timer ) == false ) {
exit( 1 );
}
printf("GO\n");
for(;;) {
// read current Time
timer.computeStartTime();
getIMUReadings( acc, baro, compass, gyro );
timer.computeTime( timeStamp );
// print data to file MATLAB: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
fprintf(logger.fp, "%d, %4d %4d %4d %.2f %.2f %.2f %.2f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %5.3f %5.3f %5.3f %5.3f %5.3f\n", ((int) timeStamp), gyro.x, gyro.y, gyro.z, compass.heading, baro.altitude, baro.pressure / 100, baro.temperature, acc.accKalman[ X ], acc.accKalman[ Y ], acc.accKalman[ Z ], acc.acc[ Z ], acc.magnitude, acc.zeroX[X], acc.zeroX[Y], acc.zeroX[Z], acc.pitch, acc.roll, acc.deriv2[X], acc.deriv2[Y], acc.deriv2[Z]);
//fprintf(logger.fp, "%d %5d %5d %5d %5.3f %5.3f %5.3f %5.3f %5.3f %5.3f\n", ((int) timeStamp), acc.raw[0], acc.raw[1], acc.raw[2], acc.acc[ 0 ], acc.acc[ 1 ], acc.acc[ 2 ], acc.accKalman[ 0 ], acc.accKalman[ 1 ], acc.accKalman[ 2 ]); //, acc.movingAverage[ 0 ], acc.movingAverage[ 1 ],acc.movingAverage[ 2 ]);
if( printTerminal == 1 ) {
printf("%d, %4d %4d %4d %.2f %.2f %.2f %.2f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %5.3f %5.3f\n", ((int) timeStamp), gyro.x, gyro.y, gyro.z, compass.heading, baro.altitude, baro.pressure / 100, baro.temperature, acc.accKalman[ X ], acc.accKalman[ Y ], acc.accKalman[ Z ], acc.acc[ Z ], acc.magnitude,acc.zeroX[X], acc.zeroX[Y], acc.zeroX[Z], acc.pitch, acc.roll);
}
// see if user wants to quit
gpioButton.getval_gpio(buttonInput);
// button pressed, so exit program
if( buttonInput == "0" ) {
printf("Exiting\n");
break;
}
timer.computeEndTime();
// delay in microseconds
delay = ( timeStep * 1000 ) - timer.getElapsedMilliseconds() * 1000;
// sleep by 'delay' in order to have periodic readings
if( delay > 0 ) {
usleep(delay);
}
// sleep 1ms
else {
usleep(1000);
}
// if blink is set to ON
if( blink == 1 ){
static float endTime;
endTime = timer.getEndTime();
blinkLED( gpioLED, endTime, lastBlink, isOnLED, 888 );
}
}
gpioLED.setval_gpio("0");
printf("bye bye\n");
return 0;
}
/*
* 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();
}