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;
}
int main(int argc, char **argv) {
printf("BMPO85 examples program\n");
I2Cdev::initialize();
BMP085 barometer ; // BMP085 class default I2C address is 0x77, specific I2C addresses may be passed as a parameter here
if ( barometer.testConnection() )
printf("BMP085 connection test successful\n") ;
else {
fprintf( stderr, "BMP085 connection test failed! exiting ...\n");
return 1;
}
barometer.loadCalibration();
float temperature;
float pressure;
float altitude;
while (true) {
barometer.setControl(BMP085_MODE_TEMPERATURE);
bcm2835_delay(5); // wait 5 ms for conversion
temperature = barometer.getTemperatureC();
barometer.setControl(BMP085_MODE_PRESSURE_3) ; //taking reading in highest accuracy measurement mode
bcm2835_delay( barometer.getMeasureDelayMicroseconds() / 1000 );
pressure = barometer.getPressure();
altitude = barometer.getAltitude(pressure);
printf(" Temperature: %3.1f deg C Pressure %3.3f kPa altitude %3.1f m\r", temperature, pressure/1000.0, altitude);
fflush(stdout);
bcm2835_delay(200);
}
return 1;
}
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;
}
void getIMUReadings( ADXL345& a, BMP085& b, HMC5883L& c, L3G4200D& g)
{
a.readAccel(); // WORKING
b.calculateAll(); // WORKING
g.computeValues(); // appears to be WORKING
c.readCompassHeading(); // appears to be WORKING
}
//------------------------------------------------------------------------------
// Loop Function - Arduino Program Loop
//------------------------------------------------------------------------------
void loop()
{
digitalWrite(13, HIGH);
delay(1000);
digitalWrite(13, LOW);
delay(100);
Serial.println("Updated numbers");
Serial.print("Temperature: ");
Serial.print(bmp.temperature(),2);
Serial.println(" deg C");
Serial.print("Pressure: ");
Serial.print(bmp.pressure());
Serial.println(" Pa");
Serial.print("Altitude: ");
Serial.print(bmp.altitude(),2);
Serial.println(" m\n");
}
//------------------------------------------------------------------------------
// Setup Function - Initializes Arduino
//------------------------------------------------------------------------------
void setup()
{
pinMode(13, OUTPUT);
Serial.begin(9600);
bmp.initialize();
// Initial
Serial.println("Initial Reading");
Serial.print("Temperature: ");
Serial.print(bmp.temperature(),2);
Serial.println(" deg C");
Serial.print("Pressure: ");
Serial.print(bmp.pressure());
Serial.println(" Pa");
Serial.print("Altitude: ");
Serial.print(bmp.altitude(),2);
Serial.println(" m\n");
// Set with current pressure
bmp.calibrateWithPres(101681);
Serial.println("Set with current pressure of 101681");
Serial.print("Temperature: ");
Serial.print(bmp.temperature(),2);
Serial.println(" deg C");
Serial.print("Pressure: ");
Serial.print(bmp.pressure());
Serial.println(" Pa");
Serial.print("Altitude: ");
Serial.print(bmp.altitude(),2);
Serial.println(" m\n");
// Set with current altitude
bmp.calibrateWithAlt(270);
Serial.println("Set with current altitude of 270");
Serial.print("Temperature: ");
Serial.print(bmp.temperature(),2);
Serial.println(" deg C");
Serial.print("Pressure: ");
Serial.print(bmp.pressure());
Serial.println(" Pa");
Serial.print("Altitude: ");
Serial.print(bmp.altitude(),2);
Serial.println(" m\n");
// Set OSS to 3
bmp.setOSS(3);
Serial.println("Set with OSS to 3");
Serial.print("Temperature: ");
Serial.print(bmp.temperature(),2);
Serial.println(" deg C");
Serial.print("Pressure: ");
Serial.print(bmp.pressure());
Serial.println(" Pa");
Serial.print("Altitude: ");
Serial.print(bmp.altitude(),2);
Serial.println(" m\n");
}
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;
}
/*
* Main Loop
*/
void loop() {
wdt_reset();
mD.vals.uslCount++; //Increment main datarecord count
AccelerometerScaled Ascaled = accel.ReadScaledAxis(); //Get Scaled Accelerometer
AccelerometerRaw Araw = accel.ReadRawAxis(); //Get Raw Accelerometer
MagnetometerScaled Mscaled = compass.ReadScaledAxis(); //Get Scaled Magnetometer
MagnetometerRaw Mraw = compass.ReadRawAxis(); //Get Raw Magnetometer
LGgyro.read(); //Get Gyro
// offset compass by hard iron
Mraw.XAxis += 40;
Mraw.YAxis += 261;
Mraw.ZAxis += 54;
//write Acc, Mag, & Gyro values to record
float AxisGs = Ascaled.XAxis;
mD.vals.AcXPayload = AxisGs * 100;
AxisGs = Ascaled.YAxis;
mD.vals.AcYPayload = AxisGs * 100;
AxisGs = Ascaled.ZAxis;
mD.vals.AcZPayload = AxisGs * 100;
mD.vals.MgXPayload = Mscaled.XAxis;
mD.vals.MgYPayload = Mscaled.YAxis;
mD.vals.MgZPayload = Mscaled.ZAxis;
mD.vals.GyXPayload = LGgyro.g.x;
mD.vals.GyYPayload = LGgyro.g.y;
mD.vals.GyZPayload = LGgyro.g.z;
//Perform tilt compensation calculation save to record
sixDOF.compCompass(Mraw.XAxis, -Mraw.YAxis, -Mraw.ZAxis, Araw.XAxis, Araw.YAxis, Araw.ZAxis, true);
float compHeading = sixDOF.atan2Int(sixDOF.xAxisComp(), sixDOF.yAxisComp());
compHeading = compHeading /100;
if (compHeading < 0 ) {
compHeading = abs(compHeading);
} else {
compHeading = 180 - compHeading + 180;
}
mD.vals.CmpssPayload = compHeading;
//get BMP085 values save to record
dps.getTemperature(&TmpPayloadFULL);
dps.getPressure(&mD.vals.PressurePayload);
mD.vals.TmpPayload = (int16_t)(TmpPayloadFULL);
mD.vals.TmpExternal = (int16_t)(sensors.getTempC(outsideThermometer)* 10);
sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperatures
//get GPS data
byte lcount = 0; //reset a loop counter
while (!NEWGPSDATA && lcount++ < 255) { //Exit the loop if we have new data or have been round it a number of times
NEWGPSDATA = feedgps();
}
if (NEWGPSDATA) { //We have new GPS data, get all of the fields we need.
int tmp_year = 0;
gps.crack_datetime(&tmp_year, &mD.vals.month, &mD.vals.day,&mD.vals.hour, &mD.vals.minute, &mD.vals.second, &mD.vals.hundredths, &mD.vals.age);
mD.vals.year = tmp_year - 2000;
if (gps.altitude() != TinyGPS_HJOE::GPS_INVALID_ALTITUDE && gps.altitude() >= 0) {
gps.get_position(&mD.vals.iLat, &mD.vals.iLong, &mD.vals.age);
mD.vals.iAlt = gps.altitude();
mD.vals.iAngle = gps.course();
mD.vals.iHspeed = gps.speed();
mD.vals.bSats = gps.satellites();
mD.vals.ihdop = gps.hdop();
}
SET_LED_Status(SET_LED_BLUE,0); //Flash blue to show we are getting GPS data
} else {
SET_LED_Status(SET_LED_GREEN,0); //Flash Green to show that we are looping but not getting GPS data
}
if(ETSerialIn.receiveData()){
}
//flip flop between I2C's to avoid both on one loop
if (SENDWIRE && (millis() - elapseSIM900) > WAIT_SIM900) {
mD.vals.tCount++;
ETI2Cout.sendData(I2C_SLV_SIM900_ADDRESS);
elapseSIM900 = millis();
}
if (!SENDWIRE && (millis() - elapseNTXB) > WAIT_NTXB) {
mD.vals.tCount++;
ETI2Cout.sendData(I2C_SLV_NTXB_ADDRESS);
elapseNTXB = millis();
//get I2C_SLV_SIM900_ADDRESS data
}
writeSDData(); //Write the data record to the SD card
SET_LED_Status(SET_LED_OFF,0); //turn off the LED
NEWGPSDATA = false; //Reset the New GPS Data flag
SENDWIRE = !SENDWIRE; //Flipflop this
}
/*
* 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();
}
