File content as of revision 1:8c4f93e10af3:

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

/**
 * Defines
 */
#define PI 3.1415926536

/* Choose PID-T1 parameters VELOCITY Controller */
//P-Part
#define P_gain   1.666666666666668//2.5
#define P_b01    1.0
#define P_b11   -0.843146028951280
#define P_a11   -0.738576714918798
//I-Part
#define I_gain   0.050505050505053
#define I_b11    1.0
#define I_a11   -1.0
//Pre-Gain
#define k        0.053348382301167*2

/* Choose P parameter POSITION Controller */
#define KpPosition  25*1.5

/* Choose setposition for the gimbal */
#define SOLLPOSITION 0 // 30° = -0.523598775598299 //[rad]


/* Assign a unique ID to the sensors */
//This board/chip uses I2C 7-bit addresses 0x19 & 0x1E & 0x6B

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(PA_0, PA_1, 115200); //921600; 115200
Serial serial1(USBTX, USBRX, 9600);

// 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 updateControllers(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()
{

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

    while(1);

  }
  if(!mag.begin())
  {

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

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

    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!");

    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
    SollStrom.period(0.001);
    //set Current to 0 Ampère
    SollStrom.write(0.5);
    //enable motor
    motorEnable.write(1);     
    
    //wait for start (user button press)
    while( SystemEnable )
    {
        serial1.printf("Wait for system enable! \r\n");
    }
    while( !SystemEnable ){}
    //attach controller loop to timer interrupt
    ControllerLoopTimer.attach(&updateControllers, 0.003030303030303f); //Assume Ts = 1/330;
    //controller on -> LED on
    SystemEnableLED = 1;
    //start timer for sample time estimation
    t.start();
    //do as long user button d'ont get pressed
    while ( SystemEnable )
    {
        //read raw sensor data
        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]);

        LoopCounter++;
        if(LoopCounter == 12000)
        {
          eMaxPos = 0;
          eMaxNeg = 0;
          serial1.printf("Error reseted");   
        }
        //serial1.printf("Roll: %f Grad \r\n",filter.getRoll());

    }
    //Stop timer
    TotalTime = t.read();
    //disable motor
    motorEnable.write(0);
    //set Current to 0 Ampère
    SollStrom.write(0.5);
    //Switch controller off
    ControllerLoopTimer.detach();
    //controller off -> LED off
    SystemEnableLED = 0;
    //Print mean loop time ->sample frequency of the madgwick-filter
    serial1.printf("Mean loop time: %f [s] \r\n", TotalTime/LoopCounter);
    serial1.printf("Max pos Fehler: %f [°] \r\n", eMaxPos/PI*180);
    serial1.printf("Max neg Fehler: %f [°] \r\n", eMaxNeg/PI*180);    
    serial1.printf("Finished!");
}



/**************************************************************************
/
/
    @brief  Regler (Geschwindigkeits- und Positionsregler)

/
**************************************************************************/
//-----------------------------------------------------------------------//
//                                                                       //
//                                                                       //
//                                     controller (PID-T1)               //
//ThetaDes[rad]  ____              ____/                                 //
//  ------>O--->| Kx |----->O---->| Cv |---> I_des [A]                   //
//         ^-    ----       ^-     ----                                  //
//         |   Pos.Cntrl.   |   Speed Cntrl.                             //
//         |                |                                            //
//         |                |                                            //
//         |                +---- Speed Omega [rad/s]                    //
//         |                                                             //
//         +--------------------- Position Theta [rad]                   //
//                                                                       //
//-----------------------------------------------------------------------//

// Cv is split into a prop. and an integrating part (limiting, windup)
//   e    ___             ____ 
//  ---->| k |---+------>| Cp |--------->O---->
//        ---    |        ----           ^
//               |   ____    ________    |
//               +->| k_I|->| z/(z-1)|---+
//                   ----    -------- \
//                                     Integrator
//
// the anti-windup loop is not shon in the diagram above! (see documentation
// for detail)
void updateControllers(void)
{
        static float ek_1 = 0;
        static float ukP_1 = 0;
        static float ukI_1 = 0;
        
        float ePosition =  SOLLPOSITION - filter.getRollRadians();
        
        //Positionsregler
        float stellgroessePosition = KpPosition*(ePosition);
        if( ePosition > eMaxPos)
        {
            eMaxPos = ePosition;
        }
        if( ePosition < eMaxNeg )
        {
            eMaxNeg =  ePosition;
        }

        //Geschwindigkeitsregler
        float ek = k*(stellgroessePosition - CorrectedGyrometer[0]);
        //P-part
        float ukP = P_gain*(P_b01*ek + P_b11*ek_1) - P_a11*ukP_1;
        //I-Part
        float stellgroesseStrom = ukP + ukI_1;
        float ukI = I_gain*I_b11*ek_1 - I_a11*ukI_1;

        //Limit +/- 0.5 Ampère
        float uSaturated;
        if(stellgroesseStrom > 0.5)
        {
            SollStrom.write( 0.8999999 );
            uSaturated = 0.5; 
        }
        else if(  stellgroesseStrom < -0.5 )
        {
            SollStrom.write( 0.1000001 );
            uSaturated = -0.5;
        }
        else
        {
            SollStrom.write(0.5f+stellgroesseStrom*0.4f/0.5f);
            uSaturated = stellgroesseStrom;
        }
        
        //Anti-Windup (back calculation)
        float deltaI = ( -stellgroesseStrom + uSaturated );
        ukI_1 = ukI + deltaI*I_gain;
        ek_1 = ek;
        ukP_1 = ukP;


}