
// 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;




/**
  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);
 // accelgyro.setXAccelOffset(0);
 // accelgyro.setYAccelOffset(0);
 // accelgyro.setZAccelOffset(0);
  Serial.println("rdy");

}

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

 
#define OL 5
#define IL 10

float tryk, temp, hojde;
int16_t mx, my, mz, ax, ay, az, gx, gy, gz;
float xx, yy, zz, fff;
float mmx[OL], mmy[OL], mmz[OL];

float fx[IL], fy[IL], fz[IL], ff[IL];
void loop()
{
  for (int i = 0; i < OL; i++) {
    mmx[i] = mmy[i] = mmz[i] = 0.0;
  }
  
  for (int o = 0; o < OL ; o++) {
     
    for (int i = 0; i < IL; i++)  {
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      fx[i] = ax; fy[i] = ay; fz[i] = az;
       
      //ff[i] = sqrt( (fx[i] * fx[i] + fy[i] * fy[i] + fz[i] * fz[i]));
      delay(100);
    }
    xx = yy = zz = fff = 0.0;
 
    for (int i = 0; i < IL; i++)  {
      xx += fx[i]; yy += fy[i]; zz += fz[i]; fff += ff[i];
    }
    xx /= IL; yy /= IL; zz /= IL; fff /= IL;
    mmx[o] = xx; mmy[o] = yy; mmz[o] = zz;
  }
  float f0 = 0, f1 = 0, f2 = 0;
  for (int i = 0 ; i < OL; i++) {
    f0 += mmx[i]; f1 += mmy[i]; f2 += mmz[i];
  }
  SP(f0 / OL); SP(" "); SP(f1 / OL); SP(" "); SP(f2 / OL); SP("\n");
  //delay(500);
   

}
