File content as of revision 2:ec30613f2b2b:

#include "SPI_9dSensor.h"
#include "SensorFusion.h"

#include <stdio.h>  // printf()
#include <signal.h> // signal()
#include <errno.h> // strerro
#include <string.h>

SPI spi2(PB_15,PB_14,PB_13);
DigitalOut sensor_CSG(PB_2);
DigitalOut sensor_CSXM(PB_1);

Serial pc(D1,D0);

int volatile interrupt = 0;

const int speed = 1875000;

unsigned char sensorG_CTRL_REG[6];
unsigned char sensorXM_CTRL_REG[9];

short int Gyro_axis_data[3]; // X, Y, Z axis
short int Gyro_axis_zero[3] = {GX_offset, GY_offset, GZ_offset};///new{-57,-10,34};///{32, -28, 138};///{25, -25, 120};
float Gyro_scale[3];

short int Acce_axis_data[3]; // X, Y, Z axis
short int Acce_axis_zero[3] = {AX_offset, AY_offset, AZ_offset};///new{-847,-420,186};///{-855, -590, 186};///{-855, -504, 186};///{-855, -454, 186};///
float Acce_scale[3];

short int Magn_axis_data[3]; //X,Y,Z axis
short int Magn_axis_zero[3] = {MX_offset, MY_offset, MZ_offset};///{-55, -21, 77};//{-32,-25,80};
float Magn_scale[3];

float B_x = 1015.0f*Magn_gain;
float B_y = 0.0f*Magn_gain;
float B_z = -580.0f*Magn_gain;
float B_total = 0.0f;

float u_cali[9] = {0,0,0,0,0,0,0,0,0};
float u_old[9] = {GX_offset, GY_offset, GZ_offset, AX_offset, AY_offset, AZ_offset, MX_offset, MY_offset, MZ_offset};

void sig_handler(int signo)
{
    if(signo == SIGINT)interrupt = 1;
}

void setup_spi(void)
{
//    signal(SIGINT, sig_handler);
//    wiringPiSetupGpio () ;
    sensor_CSG = 1;
    sensor_CSXM = 1;
    spi2.format(8, 3);
    pc.printf("SPI ready.\r\n");
    wait_ms(50);
}

void init_Sensors(void)
{
    Gyro_axis_data[0] = 0; // X
    Gyro_axis_data[1] = 0; // Y
    Gyro_axis_data[2] = 0; // Z
    Acce_axis_data[0] = 0;
    Acce_axis_data[1] = 0;
    Acce_axis_data[2] = 0;
    Magn_axis_data[0] = 0;
    Magn_axis_data[1] = 0;
    Magn_axis_data[2] = 0;
    sensorG_setup();
    sensorXM_setup();
}

void sensorG_setup(void)
{
    sensorG_CTRL_REG[1] = 0b11111111; // x, y, z enabled
    sensorG_CTRL_REG[2] = 0b00001001; // ODR = 800Hz, LPfc = 110Hz
    sensorG_CTRL_REG[3] = 0b00000000; // output circuit: push- pull
    sensorG_CTRL_REG[4] = 0b00110000; // 2000dps
    sensorG_CTRL_REG[5] = 0b00000000; // HPen = 0, LPFen = 0

    // write mode (R/W = 0)
    // Auto increase address (M/S = 1)
    //sensorG_data_write = 0x20;
    sensorG_CTRL_REG[0] = (sensorG_CTRL_REG1_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits

    // start
    sensor_CSG = 0;
    
    spi2.write(sensorG_CTRL_REG[0]);
    spi2.write(sensorG_CTRL_REG[1]);
    spi2.write(sensorG_CTRL_REG[2]);
    spi2.write(sensorG_CTRL_REG[3]);
    spi2.write(sensorG_CTRL_REG[4]);
    spi2.write(sensorG_CTRL_REG[5]);
        
    sensor_CSG = 1;
    // end
}

void sensorXM_setup(void)
{
    sensorXM_CTRL_REG[1] = 0b00000000;
    sensorXM_CTRL_REG[2] = 0b10010111; // ODR = 800Hz, x, y, z enabled
    sensorXM_CTRL_REG[3] = 0b11011000; // LPfc = 110Hz, +-8g
    sensorXM_CTRL_REG[4] = 0b00000000; // output circuit: push- pull
    sensorXM_CTRL_REG[5] = 0b00000000;
    sensorXM_CTRL_REG[6] = 0b01110100; //Magnetic data rate 100Hz
    sensorXM_CTRL_REG[7] = 0b01000000; //mag full scale +-8 gauss
    sensorXM_CTRL_REG[8] = 0b00000000; // HPen = 0, LPFen = 0
    // write mode (R/W = 0)
    // Auto increase address (M/S = 1)
    //sensorXM_data_write = sensorXM_CTRL_REG0_address;
    sensorXM_CTRL_REG[0] = (sensorXM_CTRL_REG0_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits

    // start
    sensor_CSXM = 0;
    
    spi2.write(sensorXM_CTRL_REG[0]);
    spi2.write(sensorXM_CTRL_REG[1]);
    spi2.write(sensorXM_CTRL_REG[2]);
    spi2.write(sensorXM_CTRL_REG[3]);
    spi2.write(sensorXM_CTRL_REG[4]);
    spi2.write(sensorXM_CTRL_REG[5]);
    spi2.write(sensorXM_CTRL_REG[6]);
    spi2.write(sensorXM_CTRL_REG[7]);
    spi2.write(sensorXM_CTRL_REG[8]);
        
    sensor_CSXM = 1;
    // end
}

void sensorG_read_3axis(void)
{
    static unsigned char sensorG_READ_REG[7];
    // read mode (R/W = 1)
    // Auto increase address (M/S = 1)
    //sensorG_data_write = sensorG_OUT_X_L_address;
    sensorG_READ_REG[0] = 0x28;
    sensorG_READ_REG[0] = (sensorG_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
    sensorG_READ_REG[1] = 0x00;
    sensorG_READ_REG[2] = 0x00;
    sensorG_READ_REG[3] = 0x00;
    sensorG_READ_REG[4] = 0x00;
    sensorG_READ_REG[5] = 0x00;
    sensorG_READ_REG[6] = 0x00;

    // start
    sensor_CSG = 0;
    
    spi2.write(sensorG_READ_REG[0]);
    sensorG_READ_REG[1] = spi2.write(0x00); // XL
    sensorG_READ_REG[2] = spi2.write(0x00); // XH
    sensorG_READ_REG[3] = spi2.write(0x00); // YL
    sensorG_READ_REG[4] = spi2.write(0x00); // YH
    sensorG_READ_REG[5] = spi2.write(0x00); // ZL
    sensorG_READ_REG[6] = spi2.write(0x00); // YH
    
    sensor_CSG = 1;
    // end

    // Data reconstruction
    Gyro_axis_data[0] = (short int)((sensorG_READ_REG[2]<<8) | sensorG_READ_REG[1]);
    Gyro_axis_data[1] = (short int)((sensorG_READ_REG[4]<<8) | sensorG_READ_REG[3]);
    Gyro_axis_data[2] = (short int)((sensorG_READ_REG[6]<<8) | sensorG_READ_REG[5]);
}

void sensorX_read_3axis(void)
{
    static unsigned char sensorX_READ_REG[7];

    // read mode (R/W = 1)
    // Auto increase address (M/S = 1)
    sensorX_READ_REG[0] = 0x28;//accelerator's first address
    sensorX_READ_REG[0] = (sensorX_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
    sensorX_READ_REG[1] = 0x00;
    sensorX_READ_REG[2] = 0x00;
    sensorX_READ_REG[3] = 0x00;
    sensorX_READ_REG[4] = 0x00;
    sensorX_READ_REG[5] = 0x00;
    sensorX_READ_REG[6] = 0x00;
    // start
    sensor_CSXM = 0;
    
    spi2.write(sensorX_READ_REG[0]);
    sensorX_READ_REG[1] = spi2.write(0x00); // XL
    sensorX_READ_REG[2] = spi2.write(0x00); // XH
    sensorX_READ_REG[3] = spi2.write(0x00); // YL
    sensorX_READ_REG[4] = spi2.write(0x00); // YH
    sensorX_READ_REG[5] = spi2.write(0x00); // ZL
    sensorX_READ_REG[6] = spi2.write(0x00); // YH
    
    sensor_CSXM = 1;
    // end

    // Data reconstruction
    Acce_axis_data[0] = (short int)((sensorX_READ_REG[2]<<8) | sensorX_READ_REG[1]); // X
    Acce_axis_data[1] = (short int)((sensorX_READ_REG[4]<<8) | sensorX_READ_REG[3]); // Y
    Acce_axis_data[2] = (short int)((sensorX_READ_REG[6]<<8) | sensorX_READ_REG[5]); // Z
}

void sensorM_read_3axis(void)
{
    static unsigned char sensorM_READ_REG[7];

    // read mode (R/W = 1)
    // Auto increase address (M/S = 1)
    sensorM_READ_REG[0] = 0x08;//magnetometer's first address
    sensorM_READ_REG[0] = (sensorM_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
    sensorM_READ_REG[1] = 0x00;
    sensorM_READ_REG[2] = 0x00;
    sensorM_READ_REG[3] = 0x00;
    sensorM_READ_REG[4] = 0x00;
    sensorM_READ_REG[5] = 0x00;
    sensorM_READ_REG[6] = 0x00;

    // start
    sensor_CSXM = 0;
    
    spi2.write(sensorM_READ_REG[0]);
    sensorM_READ_REG[1] = spi2.write(0x00); // XL
    sensorM_READ_REG[2] = spi2.write(0x00); // XH
    sensorM_READ_REG[3] = spi2.write(0x00); // YL
    sensorM_READ_REG[4] = spi2.write(0x00); // YH
    sensorM_READ_REG[5] = spi2.write(0x00); // ZL
    sensorM_READ_REG[6] = spi2.write(0x00); // YH
    
    sensor_CSXM = 1;
    // end

    // Data reconstruction
    Magn_axis_data[0] = (short int)((sensorM_READ_REG[2]<<8) | sensorM_READ_REG[1]); // X
    Magn_axis_data[1] = (short int)((sensorM_READ_REG[4]<<8) | sensorM_READ_REG[3]); // Y
    Magn_axis_data[2] = (short int)((sensorM_READ_REG[6]<<8) | sensorM_READ_REG[5]); // Z
}

short int filted_sensor_data(unsigned char axis_index, float freq)
{
    float data_to_filt = 0.0;

    switch(axis_index)
    {
        case 1: data_to_filt = (float)Gyro_axis_data[0]; break;
        case 2: data_to_filt = (float)Gyro_axis_data[1]; break;
        case 3: data_to_filt = (float)Gyro_axis_data[2]; break;
        case 4: data_to_filt = (float)Acce_axis_data[0]; break;
        case 5: data_to_filt = (float)Acce_axis_data[1]; break;
        case 6: data_to_filt = (float)Acce_axis_data[2]; break;
        case 7: data_to_filt = (float)Magn_axis_data[0]; break;
        case 8: data_to_filt = (float)Magn_axis_data[1]; break;
        case 9: data_to_filt = (float)Magn_axis_data[2]; break;
        default: break;
    }

    u_cali[axis_index - 1] = lpf(data_to_filt, u_old[axis_index - 1], freq);
    u_old[axis_index - 1] = u_cali[axis_index - 1];
    return (short int)u_cali[axis_index - 1];
}

void reset_gyro_offset(void)
{
    sensorG_read_3axis();
    Gyro_axis_zero[0] = filted_sensor_data(INDEX_GYRO_X, 1.8);
    Gyro_axis_zero[1] = filted_sensor_data(INDEX_GYRO_Y, 1.8);
    Gyro_axis_zero[2] = filted_sensor_data(INDEX_GYRO_Z, 1.8);
}

void reset_acceX_offset(void)
{
    sensorX_read_3axis();
    Acce_axis_zero[0] = filted_sensor_data(INDEX_ACCE_X, 1.8);
}

void get_9axis_scale(void)
{
    Gyro_scale[0] = ((float)Gyro_axis_data[0]-Gyro_axis_zero[0])*Gyro_gainx;
    Gyro_scale[1] = ((float)Gyro_axis_data[1]-Gyro_axis_zero[1])*Gyro_gainy;
    Gyro_scale[2] = ((float)Gyro_axis_data[2]-Gyro_axis_zero[2])*Gyro_gainz;
    Acce_scale[0] = ((float)(Acce_axis_data[0]-Acce_axis_zero[0]))*Acce_gainx;
    Acce_scale[1] = ((float)(Acce_axis_data[1]-Acce_axis_zero[1]))*Acce_gainy;
    Acce_scale[2] = ((float)(Acce_axis_data[2]-Acce_axis_zero[2]))*Acce_gainz;
    Magn_scale[0] = ((float)(Magn_axis_data[0]-Magn_axis_zero[0]))*Magn_gain;
    Magn_scale[1] = ((float)(Magn_axis_data[1]-Magn_axis_zero[1]))*Magn_gain;
    Magn_scale[2] = ((float)(Magn_axis_data[2]-Magn_axis_zero[2]))*Magn_gain;
}