
// Se i examples under de enkelte sensorer

#include <Wire.h>
#include <bmp085.h>  // som I lige har installeret
#include "I2Cdev.h"  // skal osse installeres
#include <MPU6050.h>
#include <HMC5883L.h>

MPU6050 accelgyro;
HMC5883L mag;

float tryk, temp, hojde;
int16_t mx, my, mz, ax, ay, az, gx, gy, gz;


/**
  Acc and Gyro:

  AFS_SEL | Full Scale Range | LSB Sensitivity
   --------+------------------+----------------
   0       | +/- 2g           | 8192 LSB/mg 16384/g
   1       | +/- 4g           | 4096 LSB/mg  8192/g
   2       | +/- 8g           | 2048 LSB/mg  4096/g
   3       | +/- 16g          | 1024 LSB/mg  2048/g
  #define MPU6050_ACCEL_FS_2          0x00
  #define MPU6050_ACCEL_FS_4          0x01
  #define MPU6050_ACCEL_FS_8          0x02
  #define MPU6050_ACCEL_FS_16         0x03



  FS_SEL | Full Scale Range   | LSB Sensitivity
   -------+--------------------+----------------
   0      | +/- 250 degrees/s  | 131 LSB/deg/s
   1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
   2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
   3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
  #define MPU6050_GYRO_FS_250         0x00
  #define MPU6050_GYRO_FS_500         0x01
  #define MPU6050_GYRO_FS_1000        0x02
  #define MPU6050_GYRO_FS_2000        0x03

  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);

  Serial.println(mpu.getFullScaleGyroRange());
  Serial.println(mpu.getFullScaleAccelRange());
**/


void setup()
{
  Serial.begin(38400);
  Wire.begin(); // I2C net til IMU board

  bmp085Init(102500.0); // ress at sealevel today
  accelgyro.initialize();
  mag.initialize();
  accelgyro.setFullScaleAccelRange(0);

}

#define SP(x) Serial.print(x)

void printData()
{
  Serial.print("temp(C) "); Serial.print(temp);
  Serial.print("\t tryk(Pa) "); Serial.print(tryk);
  Serial.print("\t hojde(m)"); Serial.println(hojde);

  SP(mx); SP(" "); SP(my); SP(" "); SP(mz); SP("\n");

  SP(ax); SP(" "); SP(ay); SP(" "); SP(az); SP("\n");
  SP(gx); SP(" "); SP(gy); SP(" "); SP(gz); SP("\n");

}

void printAcc()
{
  float norm;
  float x,y,z;
  x = ax; y = ay; z = az;
  norm = sqrt( (x*x+y*y+z*z) );
  SP(ax); SP(" , "); SP(ay); SP(" , "); SP(az);  SP(" , "); SP(norm); SP("\n");

}

void loop()
{
  bmp085Measure(&temp, &tryk, &hojde);
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  mag.getHeading(&mx, &my, &mz);

  //printData();

 printAcc();
  delay(100); // delay er maaske for kort hvis vi skal sende over radio ...
}
