File content as of revision 1:81a146dffd7a:

#include "mbed.h"
#include "LSM9DS0.h"
#include <math.h>

#define pi  3.1415926
#define g   9.81

// timer 1
#define LOOPTIME  1000                         // 1 ms
unsigned long lastMilli = 0;
// sampling time
float T = 0.001;

Timer t;

Serial uart_1(D10,D2);            // TX : D10     RX : D2           // serial 1

void init_uart(void);
void separate(void);
void uart_send(void);

float lpf(float input, float output_old, float frequency);

// uart send data
int display[6] = {0,0,0,0,0,0};
char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0};      // char num[0] , num[1] : 2 start byte

void init_sensor(void);
void real_sensor_value(void);
void sensor_fusion(void);

LSM9DS0 sensor(SPI_MODE, D9, D6);    // SPI_CS1 : D7 , SPI_CS2 : D6

int sensor_flag = 0;                 // sensor initial flag
int sensor_count = 0;

/*******************************Sensor***************************************/ 
#define Gyro_gain_x   0.00113356568421052631578947368421        
#define Gyro_gain_y   0.00113356568421052631578947368421           
#define Gyro_gain_z   0.00113356568421052631578947368421  
#define Acce_gain_x   -0.00165873343713498511154419826254  //-9.81/(max-zero)                
#define Acce_gain_y   -0.00217691902504394899561081096519                 
#define Acce_gain_z   -0.00236798520156680310444239205579 
 
/*******************************Sensor***************************************/ 
#define Gyro_gain_x   0.00113356568421052631578947368421        
#define Gyro_gain_y   0.00113356568421052631578947368421           
#define Gyro_gain_z   0.00113356568421052631578947368421  
#define Acce_gain_x   -0.00165873343713498511154419826254  //-9.81/(max-zero)                
#define Acce_gain_y   -0.00217691902504394899561081096519                 
#define Acce_gain_z   -0.00236798520156680310444239205579 
 
// 宣告從sensor讀到的值要存入處理的變數
float Gyro_axis_data[3];       // X, Y, Z axis
float Gyro_axis_data_f[3];
float Gyro_axis_data_f_old[3];
float Gyro_scale[3];           // scale = (data - zero) * gain
float Gyro_axis_zero[3] = {-3.7153254648333,-8.013402897667,-57.26524894};    
 
float Acce_axis_data[3];       // X, Y, Z axis
float Acce_axis_data_f[3];
float Acce_axis_data_f_old[3];
float Acce_scale[3];           // scale = (data - zero) * gain
float Acce_axis_zero[3] = {-840.873762375,-314.556930675,0.50990098975};
//-840.45049505  -395.32178215  4143.272277
//-841.2970297   4191.811881    1.3762376235
//5073.277228    -233.7920792   -0.356435644

// final sensor output value
float axm = 0;    
float aym = 0;   
float azm = 0;    
float u1 = 0;     
float u2 = 0;     
float u3 = 0;
// new defined direction
float ax = 0;
float ay = 0;
float az = 0;
float w1 = 0;
float w2 = 0;
float w3 = 0;
// estimated state variables
float gs1_hat = 0;
float gs1_hat_old = 0;
float gs2_hat = 0;
float gs2_hat_old = 0;
float gs3_hat = 0;
float gs3_hat_old = 0;
// normalized variables
float n = 0;               
float gs1_hat_n = 0;
float gs2_hat_n = 0;
float gs3_hat_n = 0;
//general variables
float gs1_hat_g = 0;
float gs2_hat_g = 0;
float gs3_hat_g = 0;

// bandwidth
float alpha = 6.28;     // 1Hz
float psi = 0.0;          // pitch angle
 
//Sensor Fusion 
//****************theta Angle****************************// 
float theta = 0.0;        // theta angle
float theta_old = 0.0;
float Itheta = 0.0;
float Itheta_old = 0.0;
float dtheta = 0.0;
float dtheta_old = 0.0;
float dtheta_f = 0.0;
float dtheta_f_old = 0.0;

//****************phi Angle****************************// 
float phi = 0.0;        // theta angle
float phi_old = 0.0;
float Iphi = 0.0;
float Iphi_old = 0.0;
float dphi = 0.0;
float dphi_old = 0.0;
float dphi_f = 0.0;
float dphi_f_old = 0.0;

//****************roll Angle****************************// 
float roll = 0.0;      
float roll_old = 0.0;
float roll_f = 0.0;
float roll_f_old = 0.0;
float Iroll = 0.0;      //
float Iroll_f = 0.0;
float Iroll_f_old = 0.0;
float droll = 0.0;      //roll Angle
float droll_old = 0.0;
float droll_f = 0.0;
float droll_f_old = 0.0;

//float roll_1 = 0.0;
//****************roll Angle without theta ****************************// 
float gama = 0.0;      //Roll Angle
float gama_old = 0.0;
float gama_f = 0.0;
float gama_f_old = 0.0;
float Igama  = 0.0;      
float Igama_f = 0.0;
float Igama_f_old = 0.0;
float dgama  = 0.0;      
float dgama_old = 0.0;
float dgama_f = 0.0;
float dgama_f_old = 0.0;

//****************pitch Angle****************************// 
float pitch = 0.0;      //pitch Angle
float pitch_old = 0.0;
float pitch_f = 0.0;
float pitch_f_old = 0.0;
float Ipitch = 0.0;      //
float Ipitch_f = 0.0;
float Ipitch_f_old = 0.0;
float dpitch = 0.0;      //pitch Angle
float dpitch_old = 0.0;
float dpitch_f = 0.0;
float dpitch_f_old = 0.0;
float pitch_0 = 0.0;  //

int main()
{
    t.start();

    init_uart();
    init_sensor();

    while(1) 
    {
        // timer 1
        if((t.read_us() - lastMilli) >= LOOPTIME)          // 2000 us = 2 ms
        {    
            lastMilli = t.read_us();

            // sensor initial start
            if(sensor_flag == 0) 
            {
                sensor_count++;

                if(sensor_count >= 50) 
                {
                    sensor_flag  = 1;
                    sensor_count = 0;
                }
            } 
            else 
            {
                real_sensor_value();
                sensor_fusion();
                uart_send();
            }
        }
    }   // while(1) end
}

int i = 0;
void uart_send(void)
{
    // uart send data
    display[0] = 100*pitch*180/3.1415926;
    display[1] = 100*roll*180/3.1415926;
    display[2] = 100*w3;
    display[3] = 100*gs1_hat_n;
    display[4] = 100*gs2_hat_n;
    display[5] = 100*gs3_hat_n;

    separate();

    int j = 0;
    int k = 1;
    while(k) 
    {
        if(uart_1.writeable()) 
        {            
            uart_1.putc(num[i]);
            i++;
            j++;
        }

        if(j>1)                     // send 2 bytes
        {
            k = 0;
            j = 0;
        }
    }

    if(i>13)
        i = 0;
}

void real_sensor_value(void)
{
    // sensor raw data
    Gyro_axis_data[0] = sensor.readRawGyroX();
    Gyro_axis_data[1] = sensor.readRawGyroY();
    Gyro_axis_data[2] = sensor.readRawGyroZ();

    Acce_axis_data[0] = sensor.readRawAccelX();
    Acce_axis_data[1] = sensor.readRawAccelY();
    Acce_axis_data[2] = sensor.readRawAccelZ();
    
    // gyro filter
    Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);     
    Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
    Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);     
    Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
    Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);     
    Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
        
    // acce filter
    Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);     
    Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
    Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);     
    Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
    Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);     
    Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
        
    Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x;
    Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
    Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
        
    Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
    Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
    Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
        
    // final 6 axis data       
    axm =  Acce_scale[0];
    aym =  Acce_scale[1];
    azm =  Acce_scale[2];
        
    u1  = Gyro_scale[0];
    u2  = Gyro_scale[1];
    u3  = Gyro_scale[2];
    
    ax = axm;
    ay = aym;
    az = azm;
        
    w1 = u1;
    w2 = u2;
    w3 = u3;
}

void init_uart()
{
    uart_1.baud(115200);      // 設定baudrate
}

void separate(void)
{
    num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
    num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
    num[4] = display[1] >> 8;
    num[5] = display[1] & 0x00ff;
    num[6] = display[2] >> 8;
    num[7] = display[2] & 0x00ff;
    num[8] = display[3] >> 8;
    num[9] = display[3] & 0x00ff;
    num[10] = display[4] >> 8;
    num[11] = display[4] & 0x00ff;
    num[12] = display[5] >> 8;
    num[13] = display[5] & 0x00ff;
}

void init_sensor(void)
{
    sensor.begin();
    // sensor.begin() setting :
    // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
    //                accel_scale aScl = A_SCALE_8G,
    //                mag_scale mScl = M_SCALE_8GS,
    //                gyro_odr gODR = G_ODR_760_BW_100,
    //                accel_odr aODR = A_ODR_800,
    //                mag_odr mODR = M_ODR_100);
}

void sensor_fusion(void)
{    
    gs1_hat = lpf(ax + w3*ay/alpha - w2*az/alpha , gs1_hat_old , alpha);
    gs1_hat_old = gs1_hat;
    gs2_hat = lpf(-w3*ax/alpha + ay + w1*az/alpha , gs2_hat_old , alpha);
    gs2_hat_old = gs2_hat;
    gs3_hat = lpf(w2*ax/alpha - w1*ay/alpha + az , gs3_hat_old , alpha);
    gs3_hat_old = gs3_hat;
    
    n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
    gs1_hat_n = (gs1_hat / n) * g;
    gs2_hat_n = (gs2_hat / n) * g;
    gs3_hat_n = (gs3_hat / n) * g;
    
    gs1_hat_g = cos(theta)*gs1_hat_n-sin(theta)*gs2_hat_n;
    gs2_hat_g = cos(phi)*sin(theta)*gs1_hat_n + cos(phi)*cos(theta)*gs2_hat_n - sin(phi)*gs3_hat_n;
    gs3_hat_g = sin(phi)*sin(theta)*gs1_hat_n + sin(phi)*cos(theta)*gs2_hat_n - cos(phi)*gs3_hat_n;
    
    pitch = asin(gs1_hat_g/g);
    roll = atan(gs2_hat_g/gs3_hat_g);
    gama = atan(gs2_hat_n / gs3_hat_n);//+0.0157;
    pitch_0 = asin(-gs1_hat_n/g);
    
    pitch_f = lpf(pitch, pitch_f_old, 1.0);
    pitch_f_old = pitch_f;
    Ipitch = Ipitch + pitch_f*T;
    Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0);
    Ipitch_f_old = Ipitch_f;
    dpitch = (pitch - dpitch_old)/T;
    dpitch_old = pitch;    
    dpitch_f = lpf(dpitch, dpitch_f_old, 1.0);
    dpitch_f_old = dpitch_f;
    
    roll_f = lpf(roll, roll_f_old, 1.0);
    roll_f_old = roll_f;
    Iroll = Iroll + roll_f*T;
    Iroll_f = lpf(Iroll, Iroll_f_old, 18.0);
    Iroll_f_old = Iroll_f;
    droll = (roll - droll_old)/T;
    droll_old = roll;   
    droll_f = lpf(droll, droll_f_old, 1.0);
    droll_f_old = droll_f;
    
    gama_f = lpf(gama, gama_f_old, 1.0);
    gama_f_old = gama_f;
    Igama = Igama + gama_f*T;
    Igama_f = lpf(Igama, Igama_f_old, 18.0);
    Igama_f_old = Igama_f;
    dgama = (gama - dgama_old)/T;
    dgama_old = gama;   
    dgama_f = lpf(dgama, dgama_f_old, 1.0);
    dgama_f_old = dgama_f;
      
}

float lpf(float input, float output_old, float frequency)
{
    float output = 0;
    
    output = (output_old + frequency*T*input) / (1 + frequency*T);
    
    return output;
}