/* (C) Jens Dalsgaard Nielsen, AAU, 2014 * Free to use, copy modify etc * NO warranty whatsoever * If you find it usefull then I will be happy :-) */ #include #include /* DY80 komponenenter */ #include #include #include #include // dagens luftryk i 0 m i Pascal saa vi kan regne hojde udfra lufttryk #define ZEROPRESS 101400 // hvis radio skal printe halve info hver anden gang #define HALFRADIO hmc5883l compass; adxl345 adxl; /* Variable til kompas */ MagnetometerScaled scaled; MagnetometerRaw raw; int MilliGauss_OnThe_XAxis; float heading; float declinationAngle; float headingDegrees; TinyGPSPlus gps; float temperature, pressure, atm, altitud; /* variable for trykmaaler */ int gyrox, gyroy, gyroz;/* og for gyro */ int accx, accy,accz; /* variable for accelerometer */ unsigned long t1=0,t2=0; /* for loop */ int radioloop=0; unsigned int samplenr=0; char toogle = 0; // ----------------oOo------------------- void setup() { Serial.begin(1200); // seriel interface Serial3.begin(38400); // openlog - se i CONFIG.TXT paa sd kort Wire.begin(); // start i2c bus - forbindelse til dy80 board init_pressure(); // bmp085 sensor init_acc(); // adxl345 sensor init_compass(); // hm5883 sensor init_gyro(); // l3g4200 init_GPS(); // og GPS gps_empty(); // og toem lige input buffer } void loop() { /* get time and let this inner loop run every xxx msec */ t2 = millis(); if (500 <= (t2-t1)) // er der gaaet 1/2 sekund saa gogo { t1 = t2; // saa vi husker forrige tid_log do_measurements(); print_log(); /* Det samme for radio, men kun hver 5. gang da radio kun kører 1200 baud */ radioloop++; if (radioloop > 5) { radioloop=0; // reset counter print_radio(); } } /* GPS - traek data hjem fra GPS - skal gores hele tiden */ while (Serial1.available() > 0) { gps.encode(Serial1.read()); } } void do_measurements() { bmp085_meas(&temperature, &pressure, &altitud); adxl.readAccel(&accx, &accy, &accz); /* Accelerometer - gem i accx, accy,accz*/ l3g4200DGetGyroValues(&gyrox,&gyroy,&gyroz); /* gyro - gem i gyrox,... This will update x, y, and z with new values */ hmc5883mea(); /* COMPASS */ } void print_log() { Serial3.print(samplenr); Serial3.print(" "); printDateTime_log(gps.date, gps.time); // 2 tal (dato kl slet) bmp085_print_log(); // 3 tal - temp(C) Pres (Pa) højde(m) adxl345_print_log(); // 3 tal acc x,y,z i mG ( 1 G ~ 9.82 m/s2) l3g4200_print_log(); // 3 tal gyrox,y,z hmc5883_print_log(raw, scaled, heading, headingDegrees); // 8 tal n´mag x,y,z raw, x,y,z skaleret, heading (heading (degr)) gps_print_log(); // 3 tal - lat lon altitude(hojde) (m) Serial3.println(" "); // linieskift } void print_radio() { Serial.print(samplenr); Serial.print(" "); #ifdef HALFRADIO /* to save bandwith we print half each time */ if (toogle) { toogle=0; bmp085_print_radio(); adxl345_print_radio(); l3g4200_print_radio(); // hmc5883_print_radio(raw, scaled, heading, headingDegrees); // Output the data via the serial port. // gps_print_radio(); } else { toogle = 1; // bmp085_print_radio(); // adxl345_print_radio(); // l3g4200_print_radio(); hmc5883_print_radio(raw, scaled, heading, headingDegrees); // Output the data via the serial port. gps_print_radio(); } #else // print it all bmp085_print_radio(); adxl345_print_radio(); l3g4200_print_radio(); hmc5883_print_radio(raw, scaled, heading, headingDegrees); // Output the data via the serial port. gps_print_radio(); #endif Serial.println(" "); } void bmp085_meas(float *temperature, float * pressure, float * altitud) { *temperature = bmp085GetTemperature(bmp085ReadUT()); //MUST be called first *pressure = bmp085GetPressure(bmp085ReadUP()); *altitud = calcAltitude(*pressure,ZEROPRESS); //Uncompensated caculation - in Meters } void bmp085_print_log() { Serial3.print(temperature); Serial3.print(" "); Serial3.print(pressure); Serial3.print(" "); Serial3.print(altitud); Serial3.print(" "); } void bmp085_print_radio() { Serial.print(temperature); Serial.print(" "); Serial.print(pressure); Serial.print(" "); Serial.print(altitud); Serial.print(" "); } void adxl345_print_log() { Serial3.print(accx*3.9); Serial3.print(" "); // 3.9 mg /LSB (dvs pr bit saa ren skalering) Serial3.print(accy*3.9); Serial3.print(" "); Serial3.print(accz*3.9); Serial3.print(" "); } void adxl345_print_radio() { Serial.print(accx*3.9); Serial.print(" "); // 3.9 mg /LSB (dvs pr bit saa ren skalering) Serial.print(accy*3.9); Serial.print(" "); Serial.print(accz*3.9); Serial.print(" "); } void hmc5883_print_radio(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees) { Serial.print(raw.XAxis); Serial.print(" "); Serial.print(raw.YAxis); Serial.print(" "); Serial.print(raw.ZAxis); Serial.print(" "); Serial.print(scaled.XAxis); Serial.print(" "); Serial.print(scaled.YAxis); Serial.print(" "); Serial.print(scaled.ZAxis); Serial.print(" "); Serial.print(heading); Serial.print(" "); Serial.print(headingDegrees); Serial.print(" "); } void hmc5883_print_log(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees) { Serial3.print(raw.XAxis); Serial3.print(" "); Serial3.print(raw.YAxis); Serial3.print(" "); Serial3.print(raw.ZAxis); Serial3.print(" "); Serial3.print(scaled.XAxis); Serial3.print(" "); Serial3.print(scaled.YAxis); Serial3.print(" "); Serial3.print(scaled.ZAxis); Serial3.print(" "); Serial3.print(heading); Serial3.print(" "); Serial3.print(headingDegrees); Serial3.print(" "); } void l3g4200_print_radio() { Serial.print(gyrox); Serial.print(" "); Serial.print(gyroy); Serial.print(" "); Serial.print(gyroz); Serial.print(" "); } void l3g4200_print_log() { Serial3.print(gyrox); Serial3.print(" "); Serial3.print(gyroy); Serial3.print(" "); Serial3.print(gyroz); Serial3.print(" "); } void gps_print_radio() { Serial.print(gps.location.lat(),4); Serial.print(" "); Serial.print(gps.location.lng(),4); Serial.print(" "); Serial.print(gps.altitude.meters()); Serial.print(" "); } void gps_print_log() { Serial3.print(gps.location.lat(),4); Serial3.print(" "); Serial3.print(gps.location.lng(),4); Serial3.print(" "); Serial3.print(gps.altitude.meters()); Serial3.print(" "); } void gps_empty() { char c; while (Serial1.available() > 0) c= Serial1.read(); } static void printDateTime_radio(TinyGPSDate &d, TinyGPSTime &t) { if (!d.isValid()) { Serial.print("* "); } else { char sz[32]; sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year()); Serial.print(sz); } if (!t.isValid()) { Serial.print("* "); } else { char sz[32]; sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second()); Serial.print(sz); } } static void printDateTime_log(TinyGPSDate &d, TinyGPSTime &t) { if (!d.isValid()) { Serial3.print("* "); } else { char sz[32]; sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year()); Serial3.print(sz); } if (!t.isValid()) { Serial3.print("* "); } else { char sz[32]; sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second()); Serial3.print(sz); } } void hmc5883mea() { // tror nok maalinger er milli gauss // Retrive the raw values from the compass (not scaled). raw = compass.ReadRawAxis(); // Retrived the scaled values from the compass (scaled to the configured scale). scaled = compass.ReadScaledAxis(); // Values are accessed like so: MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis) // Calculate heading when the magnetometer is level, then correct for signs of axis. heading = atan2(scaled.YAxis, scaled.XAxis); // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location. // Find yours here: http://www.magnetic-declination.com/ // Mine is: 2� 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457 // If you cannot find your Declination, comment out these two lines, your compass will be slightly off. declinationAngle = 0.0; // Jens 0.0457; heading += declinationAngle; // Correct for when signs are reversed. if(heading < 0) heading += 2*PI; // Check for wrap due to addition of declination. if(heading > 2*PI) heading -= 2*PI; // Convert radians to degrees for readability. headingDegrees = heading * 180/M_PI; // Normally we would delay the application by 66ms to allow the loop // to run at 15Hz (default bandwidth for the HMC5883L). // However since we have a long serial out (104ms at 9600) we will let // it run at its natural speed. // delay(66); // Output the data down the serial port. } void init_gyro() { l3g4200DSetup(500); // Configure L3G4200 - 250, 500 or 2000 deg/sec } void init_acc() { adxl.powerOn(); } void init_compass() { int error; compass = hmc5883l(); // Construct a new HMC5883 compass. error = compass.SetScale(1.3); // Set the scale of the compass. error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous } void init_pressure() { bmp085Calibration(); } void init_GPS() { Serial1.begin(9600); }