File content as of revision 3:cc828af4a4c6:

#include "mbed.h"
#define _MBED_
#include "Adafruit_9DOF.h"
#include "Serial_base.h"
#include "MadgwickAHRS.h"

/**
 * Defines
 */
#define PI 3.1415926536


Adafruit_9DOF                 dof   = Adafruit_9DOF();

Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);

Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);

Adafruit_L3GD20_Unified gyro        = Adafruit_L3GD20_Unified(30303);

//Debug/Communication with PC through
Serial pc(SERIAL_TX, SERIAL_RX,115200);        // serial connection via USB - programmer

// Magnetometer correction
// mounted in gimbal

//Cal for Nickachse
//double RotMag [3][3]     = {{0.947568, -0.0262851, 0.0355492}, 
//                           {-0.0262851, 0.98413,   0.014105}, 
//                            {0.0355492, 0.014105,  0.970769}};
                         
//double offsetsMag [3]    = {   -22.1205, -13.9738, -2.55159   };

double offsetsMag[3] = {23.5297, -7.51693, 17.5943};
double RotMag[3][3] = {{0.678312, -0.0371453, 0.0139236}, {-0.0371453, 0.993299, 0.0229827}, {0.0139236, 0.0229827, 0.809984}};

//double gainsMag [3]      = {    39.1659,  38.1534,   35.7245   };
//double refMag            =  47.9824; //nT

//Accelerometer correction

double RotAccel [3][3]     = { {-0.1251,   -0.7810,   -0.6118},
                              { 0.0409,    0.6121,   -0.7879},
                              { 0.9913,   -0.1238,   -0.0466}  };  
                         
                         
double offsetsAccel [3]    = {   -0.215747,   0.509946,   -0.235360   };
double gainsAccel   [3]    = {    0.978548,   0.976637,    0.939921   };


//Gyrometer correction
double offsetsGyro [3]     = { -0.0107709,     0.00729576,   0.0225833   };

//Filter for orientation estimation
Madgwick filter;

DigitalIn CPUEnable(D11);
DigitalOut motorEnable(D12, 0);
PwmOut SollStrom(PB_3);    //in [A]

//Timer for controller interrupt
// the controll loop will be atached to the timer interrupt
Ticker  ControllerLoopTimer;
void updateAHRS(void);

//Sensor events, contains the last read sensor values
sensors_event_t mag_event;
sensors_event_t accel_event;
sensors_event_t gyro_event;

//Sensor values after the calibration/correction is applied, (x,y,z)
double CorrectedMagnetometer[3];
double CorrectedAccelerometer[3];
double CorrectedGyrometer[3];

//"User Interface", Start/Stop controller
DigitalIn SystemEnable(USER_BUTTON);
//Status LED, on when controller on
DigitalOut SystemEnableLED(LED3, 0);

//variables for loop time estimation
uint32_t LoopCounter = 0;
Timer t;
float TotalTime;

//maximaler Fehler
float eMaxPos = 0;
float eMaxNeg = 0;

/**************************************************************************
/
/
    @brief  Initialises all the sensors used by this example

/
**************************************************************************/

void initSensors()
{
 pc.printf(("Init...\r\n"));
  if(!accel.begin())
  {
 
   /* There was a problem detecting the LSM303 ... check your connections */
    
    pc.printf(("Ooops, no LSM303 detected ... Check your wiring!\r\n"));

    while(1);

  }
  if(!mag.begin())
  {

    /* There was a problem detecting the LSM303 ... check your connections */

    pc.printf("Ooops, no LSM303 detected ... Check your wiring!\r\n");

    while(1);

  }
  
  gyroRange_t rng = GYRO_RANGE_2000DPS; //set gyro range to +/-2000dps
  
  if(!gyro.begin(rng))
  {

    /* There was a problem detecting the LSM303 ... check your connections */

    pc.printf("Ooops, no L3GD20 detected ... Check your wiring!\r\n");

    while(1);

  }

}
/**************************************************************************

    @brief  system initialisation


**************************************************************************/

void setup(void) 
{
  pc.printf(("Adafruit 9 DOF Pitch/Roll/Heading Example \n"));
  pc.printf("\n");
  
  /* Initialise the sensors */
  initSensors();

}


int main()
{
    //init system
    setup();      
    
    //set PWM frequncy to 1 kHZ
    wait_ms(500);
    pc.printf("Started! \r\n");
    //ControllerLoopTimer.attach(&updateControllers, 1.0f); //Assume Ts = 1/100;
    //controller on -> LED on
    t.start();
    //do as long user button d'ont get pressed
    while ( 1 )// schafft etwa 400Hz
    {
        LoopCounter++;
        updateAHRS();
        if(LoopCounter == 100)
        {
          LoopCounter = 0;
         //pc.printf("R %3.2f P %3.2f Y %3.2f \r\n",filter.getRoll(),filter.getPitch(),filter.getYaw());
         //pc.printf("R %2.4f P %2.4f Y %2.4f \r\n",gyro_event.gyro.x,gyro_event.gyro.y,gyro_event.gyro.z);
         filter.getRoll();
         pc.printf("A %f B %f C %f D %f\r\n",filter.Q[0],filter.Q[1],filter.Q[2],filter.Q[3]);
        }
        //wait_ms(100);

    }
}


void updateAHRS(void)
{
       
        mag.getEvent(&mag_event);
        accel.getEvent(&accel_event);
        gyro.getEvent(&gyro_event);
        
        //Magnetometer correction
        mag_event.magnetic.x = ((double)mag_event.magnetic.x) - offsetsMag[0];
        mag_event.magnetic.y = ((double)mag_event.magnetic.y) - offsetsMag[1];
        mag_event.magnetic.z = ((double)mag_event.magnetic.z) - offsetsMag[2];
          
        CorrectedMagnetometer[0] = ((double)mag_event.magnetic.x)*RotMag[0][0]  +  ((double)mag_event.magnetic.y)*RotMag[0][1]  +  ((double)mag_event.magnetic.z)*RotMag[0][2];
        CorrectedMagnetometer[1] = ((double)mag_event.magnetic.x)*RotMag[1][0]  +  ((double)mag_event.magnetic.y)*RotMag[1][1]  +  ((double)mag_event.magnetic.z)*RotMag[1][2];
        CorrectedMagnetometer[2] = ((double)mag_event.magnetic.x)*RotMag[2][0]  +  ((double)mag_event.magnetic.y)*RotMag[2][1]  +  ((double)mag_event.magnetic.z)*RotMag[2][2];

        
        //Accelerometer correction
        CorrectedAccelerometer[0] = ((double)accel_event.acceleration.x) - offsetsAccel[0];
        CorrectedAccelerometer[1] = ((double)accel_event.acceleration.y) - offsetsAccel[1];
        CorrectedAccelerometer[2] = ((double)accel_event.acceleration.z) - offsetsAccel[2];
        
        CorrectedAccelerometer[0] = CorrectedAccelerometer[0] * gainsAccel[0];
        CorrectedAccelerometer[1] = CorrectedAccelerometer[1] * gainsAccel[1];
        CorrectedAccelerometer[2] = CorrectedAccelerometer[2] * gainsAccel[2];
        
        
        //Gyrometer correction
        CorrectedGyrometer[0] = ((double)gyro_event.gyro.x)  - offsetsGyro[0];
        CorrectedGyrometer[1] = ((double)gyro_event.gyro.y)  - offsetsGyro[1];
        CorrectedGyrometer[2] = ((double)gyro_event.gyro.z)  - offsetsGyro[2];
        
        //Perform Madgwick-filter update
        filter.update(   CorrectedGyrometer[0]/ PI * 180, -CorrectedGyrometer[1]/ PI * 180, -CorrectedGyrometer[2]/ PI * 180 ,
                        -CorrectedAccelerometer[0] ,       CorrectedAccelerometer[1] ,       CorrectedAccelerometer[2] ,
                         CorrectedMagnetometer[0] ,       -CorrectedMagnetometer[1],        -CorrectedMagnetometer[2]); 
    
}