File content as of revision 2:e28f4414ca2c:

/* MPU9150 Basic Example Code
 by: Kris Winer
 date: April 1, 2014
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic MPU-9150 functionality including parameterizing the register addresses, initializing the sensor, 
 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
 
 SDA and SCL should have external pull-up resistors (to 3.3V).
 10k resistors are on the EMSENSR-9250 breakout board.
 
 Hardware setup:
 MPU9150 Breakout --------- Arduino
 VDD ---------------------- 3.3V
 VDDI --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND
 
 Note: The MPU9150 is an I2C sensor and uses the Arduino Wire library. 
 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 */

#include "mbed.h"
#include "MPU9150.h"
#include "C12832.h"
#include "MSCFileSystem.h"
#include "BMP180.h"
#include "DW1000.h"                                 
#include "MM2WayRanging.h"  
#include "stdlib.h"

#define LAST(k, n) ((k)&((1<<(n))-1))
#define MID(k,m,n) LAST((k)>>(m),((n)-(m)))
#define myprintf pc.printf 

float sum = 0;
uint32_t sumCount = 0, mcount = 0;
char buffer[14];

    //IMU
    MPU9150 MPU9150;
    
    //BARO
    BMP180 bmp180(p28, p27);                    // sda, scl
    
    // DWM1000
    DW1000          dw(p5, p6, p7, p11, p15);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
    MM2WayRanging   node(dw);                   // Ranging algorithm
    
    //USB 
    MSCFileSystem fs("fs");
    
    //Timer
    Timer t, timer;
   
    // Application System
    Serial pc(USBTX, USBRX); // tx, rx
    
    // joystick
    BusIn joystick(p15,p12,p13,p16);
   
    // LCD display
    //C12832 lcd(p5, p7, p6, p8, p11);
    
    // joystick
    DigitalIn fire(p14);
        
int main()
{
  pc.baud(9600);  

  //Set up I2C
  i2c.frequency(400000);  // use fast (400 kHz) I2C  
  
  //lcd.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
  
  t.start();        
  
  // Connection Test of the DecaWave Ranging Measurement Unit 
    
    dw.setEUI(0xFAEDCD01FAEDCD01);                                  // basic methods called to check if we have a working SPI connection
    if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130)) //&& dw2.getEUI() == 0xFAEDCD01FAEDCD01 && dw2.getDeviceID() == 0xDECA0130))
    { myprintf("\n\rDWM1000 Identification error:\n DeviceID1 = %X\n EUI1 = %016llX", dw.getDeviceID(), dw.getEUI()); //dw2.getDeviceID(), dw2.getEUI());
    while(1);}
    else 
    {myprintf("DWM100 Connection established\n\r");}

    // Anchor or Beacon?
   
        node.isAnchor = false;
        node.address = 1;
        myprintf("This node is Beacon1      address: %d \r\n", node.address);
        
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150);  // Read WHO_AM_I register for MPU-9250
  
  if (whoami == 0x68) // WHO_AM_I should be 0x68
  {
    myprintf("IMU Connection established, Initialization...\n\r");
    MPU9150.MPU9150SelfTest(SelfTest);      // Accelerometer and gyroscope self test; check calibration wrt factory settings
    //lcd.printf("x-axis self test: acceleration trim within %f % of factory value\n\r", SelfTest[0]);
    
    MPU9150.resetMPU9150(); // Reset registers to default in preparation for device calibration
    MPU9150.calibrateMPU9150(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
    //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
    
    MPU9150.initMPU9150(); 
    myprintf("MPU9150 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
    MPU9150.initAK8975A(magCalibration);
    myprintf("AK8975 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
   }
   else
   {
    myprintf("Could not connect to MPU9150: \n\r");
    myprintf("%#x \n",  whoami);
    while(1) ; // Loop forever if communication doesn't happen
    }

    bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling
      
    uint8_t MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values
    MPU9150.getAres(); // Get accelerometer sensitivity
    MPU9150.getGres(); // Get gyro sensitivity
    mRes = 10.*1229./4096.; // Conversion from binary to microtesla and from 1229 microTesla full scale (4096) to 12.29 Gauss full scale
    // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
    // like the gyro and accelerometer biases
    magbias[0] = -5.;   // User environmental x-axis correction in milliGauss
    magbias[1] = -95.;  // User environmental y-axis correction in milliGauss
    magbias[2] = -260.; // User environmental z-axis correction in milliGauss

FILE *fic=  fopen("/fs/test.txt","w");
int button = 0;
int a = -2; 
int First_Start = 1;
float Start;
float tnodetoanchor2, tnodetoanchor3, tnodetoanchor4, tnode2toanchor4;

int Points = 0;
 while(joystick.read() != 4) {
  // Get Ranging measurements 
  node.requestRanging(2);
  tnodetoanchor2 = t.read();  
  
  node.requestRanging(3);
  tnodetoanchor3 = t.read();  

  node.requestRanging(4);
  tnodetoanchor4 = t.read();  
  
  //node2.requestRanging(2);
  //tnode2toanchor2 = t.read();
  
  //node2.requestRanging(4);
  //tnode2toanchor4 = t.read();
  
   float temps;
  
  // If intPin goes high, all data registers have new data
  if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt

    MPU9150.readAccelData(accelCount);  // Read the x/y/z adc values   
    // Now we'll calculate the accleration value into actual g's
    ax = (float)accelCount[0]*aRes;  - accelBias[0];  // get actual g value, this depends on scale being set
    ay = (float)accelCount[1]*aRes;  - accelBias[1];   
    az = (float)accelCount[2]*aRes;  - accelBias[2];  
   temps = t.read();
   
    MPU9150.readGyroData(gyroCount);  // Read the x/y/z adc values
    // Calculate the gyro value into actual degrees per second
    gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
    gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
  
    mcount++;
    //if (mcount > 200/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below) 
    MPU9150.readMagData(magCount);  // Read the x/y/z adc values
    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental corrections
    mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
    my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
    mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
    mcount = 0;
    //}
  }
  
   
    Now = t.read_us();
    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
    lastUpdate = Now;
    
    sum += deltat;
    sumCount++;
    
//    if(lastUpdate - firstUpdate > 10000000.0f) {
//     beta = 0.04;  // decrease filter gain after stabilized
//     zeta = 0.015; // increasey bias drift gain after stabilized
 //   }
    
   // Pass gyro rate as rad/s
  //MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
  //MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

    // Serial print and/or display at 0.5 s rate independent of data rates
    
    //myprintf("\r\n  CIR= %u, %016llX", MID(dw.getCIR_PWR(),48,64), dw.getCIR_PWR());
    //myprintf("\r\n RXPACC = %u, %X", MID(dw.getRXPACC(), 20,32), dw.getRXPACC());


    //while(1);
    
    delt_t = t.read_ms() - count;
    timer.start();
        if (fire)
        { Start = timer.read();
          myprintf("#");
          fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d\n", a, a, a, a, a, a, a, a, a, a, a, a, a);
          wait(0.4);}
        
        if (Start>0.01)
        { button = 1;
          Points = 0;
          myprintf("\n\rNouvelle Aquisition");
          wait(1);}
        
        if (fic != NULL && button && Points < 2000)//&& bmp180.ReadData(&temperature, &pressure)
        {
         fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n",temps, 1000*ax, 1000*ay, 1000*az, gx, gy, gz,tnodetoanchor2, node.distances[2],tnodetoanchor3, node.distances[3],tnodetoanchor4,node.distances[4],mx,my,mz);
         myprintf("%f       %f    %f   \n\r",node.distances[2], node.distances[3], node.distances[4]);// Points, node.distances[2],node.distances[3],node.distances[4],1000*ax, 1000*ay, 1000*az );
         //fprintf(fic, ";%u;%u;%u", MID(dw.getRXPACC(), 20,32),dw.getFP_AMPL1(), MID(dw.getCIR_PWR(), 16, 32));
         //fprintf(fic, ";%u;%.2f", MID(dw.getCIR_PWR(), 32, 48), node.distances[2]);
         Start = 0;
         Points ++;} 
        else              
              {myprintf(".");
               Start = 0;}

    if (delt_t > 500) { // update LCD once per half-second independent of read rate        
    
    tempCount = MPU9150.readTempData();  // Read the adc values
    temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
    //myprintf(" temperature = %f  C", temperature); 
    //myprintf("Ranging = %f\n\r ", Distance);
    
    //myprintf("q0 = %f\n\r", q[0]);
    //myprintf("q1 = %f\n\r", q[1]);
    //myprintf("q2 = %f\n\r", q[2]);
    //myprintf("q3 = %f\n\r", q[3]);      
    
/*    lcd.clear();
    lcd.printString("MPU9150", 0, 0);
    lcd.printString("x   y   z", 0, 1);
    sprintf(buffer, "%d %d %d mg", (int)(1000.0f*ax), (int)(1000.0f*ay), (int)(1000.0f*az));
    lcd.printString(buffer, 0, 2);
    sprintf(buffer, "%d %d %d deg/s", (int)gx, (int)gy, (int)gz);
    lcd.printString(buffer, 0, 3);
    sprintf(buffer, "%d %d %d mG", (int)mx, (int)my, (int)mz);
    lcd.printString(buffer, 0, 4); 
 */  
  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  // In this coordinate system, the positive z-axis is down toward Earth. 
  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    //yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    //roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    //pitch *= 180.0f / PI;
    //yaw   *= 180.0f / PI; 
    //yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    //roll  *= 180.0f / PI;

    //myprintf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
    //myprintf("average rate = %f\n\r", (float) sumCount/sum);
//    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
//    lcd.printString(buffer, 0, 4);
//    sprintf(buffer, "rate = %f", (float) sumCount/sum);
//    lcd.printString(buffer, 0, 5);
    
    myled= !myled;
    count = t.read_ms(); 

    if(count > 1<<21) {
        t.start(); // start the timer over again if ~30 minutes has passed
        count = 0;
        deltat= 0;
        lastUpdate = t.read_us();
        
        }
    sum = 0;
    sumCount = 0; 
}
}
fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d", a, a, a, a, a, a, a, a, a, a, a, a, a);
fclose(fic);
 
 }