File content as of revision 1:f2d1cdd67064:

#include "MTi2.h"
#include <stdio.h>
#include "mbed.h"
#include <math.h>
#include <stdlib.h>

#define GRAVITYACCELERATION 9.81f
#define pi 3.14159265358979323846f
SPI spi_MTI(PB_15, PB_14, PB_13);//MOSI MISO SCLK
Serial pc(USBTX,USBRX);
InterruptIn mybutton(USER_BUTTON);
Ticker main_function; //interrupt

DigitalOut led1(LED1);
DigitalOut cs_MTI(PC_4);

typedef union{
    uint32_t data1;
    float data2;
}imu_data;

imu_data eul[3];//euler angle
imu_data acc[3];
imu_data gry[3];

bool button_state = false;
float dt = 0.01; // sec

void step_command();
void Start_read();

int i = 1;


int main(void)
{
    spi_MTI.format(8, 3);
    pc.baud(230400);
    MTi2_Init();
    mybutton.fall(&step_command);
    
    main_function.attach_us(&Start_read,dt*1000000);
    while (1) {
        //LL_mDelay(5);
        //wait_us(5000); // ?

    }

}

void Start_read()                      //interrupt function by TT
{
    if (button_state == true)
    {
        ReadData();
        // printf("euler  %f,%f,%f\r\n",euler[0],euler[1],euler[2]);
        //printf("%f,%f,%f,%f,%f,%f\r\n",accel[0],accel[1],accel[2],omega[0],omega[1],omega[2]);
        printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],accel_[0],accel_[1],accel_[2],omega[0],omega[1],omega[2]);
        //printf("omega  %f,%f,%f\r\n",omega[0],omega[1],omega[2]);
    }
}


void step_command(){
    led1 = !led1;
    button_state = !button_state;
}

void SendOpcode(uint8_t Opcode)
{
//    printf("SendOpcode \r\n");
    //LL_SPI_TransmitData8(SPI3, Opcode);
    //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
    //FW[0] = LL_SPI_ReceiveData8(SPI3);
    FW[0] = spi_MTI.write(Opcode);
    
    for(uint8_t i = 0;i<3;i++){// 3 fillword ?
        //LL_SPI_TransmitData8(SPI3, i);
        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
        //FW[i+1] = LL_SPI_ReceiveData8(SPI3);
        FW[i+1] = spi_MTI.write(i);
    }
}

uint8_t ReadProtInfo(){
    len = 2;
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
    SendOpcode(ProtInfo);//send opcode
    for(int i = 0;i<len;i++){//read data
        //LL_SPI_TransmitData8(SPI3, 0x00);
        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
        buffer[i] = spi_MTI.write(0x00);
    }
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
    if(FW[0]!=0xFA||FW[1]!=0xFF||FW[2]!=0xFF||FW[3]!=0xFF){
        printf("Error!!\n");
    }
    return buffer[1];
}

void ConfigureProt(_Bool M,_Bool N,_Bool O,_Bool P)
{
//    printf("ConfigureProt \r\n");
    uint8_t config = (M<<3) | (N<<2) | (O<<1) | (P<<0);
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
    SendOpcode(ConfigProt);
    //LL_SPI_TransmitData8(SPI3, config);
    spi_MTI.write(config);
    //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
    //LL_SPI_ReceiveData8(SPI3);   ???
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
}

void PipeStatus(){
//    printf("PipeStatus \r\n");
    len = 4;
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
    SendOpcode(PipeStat);//send opcode
    for(int i = 0;i<len;i++){//read data
        //LL_SPI_TransmitData8(SPI3, 0x00);
        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
        buffer[i] = spi_MTI.write(0x00);
    }
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
    notificationSize = buffer[0] | (buffer[1]<<8);
    measurementSize = buffer[2] | (buffer[3]<<8);
//    printf("AAAAAAAAAAAAAAAAA %d\r\n",measurementSize);
//    printf("nSize:%d\r\n",notificationSize);
//    printf("mSize:%d\n",measurementSize);
}

void NotificationPipe(){
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
    SendOpcode(NotiPipe);//send opcode
    for(int i = 0;i<notificationSize;i++){//read data
        //LL_SPI_TransmitData8(SPI3, 0x00);
        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
        buffer[i] = spi_MTI.write(0x00);
    }
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
}

void MeasurementPipe(){
//    printf("MeasurementPipe \r\n");
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
    SendOpcode(MeasPipe);//send opcode
    for(int i = 0;i<measurementSize;i++){//read data
        //LL_SPI_TransmitData8(SPI3, 0x00);
        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
        buffer[i] = spi_MTI.write(0x00);
    }
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
}

void ControlPipe(){
    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
    cs_MTI = 0;
        SendOpcode(Control);//send opcode
        for(int i = 0;i<ctrl_len;i++){//read data
            //LL_SPI_TransmitData8(SPI3, ctrlBuf[i]);
            //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
            //buffer[i] = LL_SPI_ReceiveData8(SPI3);
            buffer[i] = spi_MTI.write(ctrlBuf[i]);
        }
    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
    cs_MTI = 1;
}

void ReadData(){
//    printf("ReadData \r\n");
    PipeStatus();
    //LL_mDelay(1);
    wait_us(100);
    MeasurementPipe();
    int len1,len2,len3,data_bytes;
//    printf("Measurement FINISH \r\n");
//    printf("buffer[0] == %d \r\n",buffer[0]);
    if(buffer[0] == 0x36){
//        printf("buffer \r\n");
        if(buffer[2]== 0x20&&buffer[3]== 0x30){//Read Euler Angle
            len1 = buffer[4];
            data_bytes = len1/3;
            for(int j=0;j<3;j++){
                uint32_t temp = (buffer[5+j*data_bytes]<<24) | (buffer[6+j*data_bytes]<<16) | (buffer[7+j*data_bytes]<<8) | (buffer[8+j*data_bytes]);
//                float *ptr = NULL;
//                ptr = &temp;
                eul[j].data1 = temp;
//                euler[j] = *ptr;
                euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
            }
        }
        if(buffer[4+len1+1]== 0x40&&buffer[4+len1+2]== 0x20){
            len2 = buffer[4+len1+3];
            data_bytes = len2/3;
            for(int j=0;j<3;j++){
//                printf("A\r\n");
                uint32_t temp = (buffer[8+len1+j*data_bytes]<<24) | (buffer[9+len1+j*data_bytes]<<16) | (buffer[10+len1+j*data_bytes]<<8) | (buffer[11+len1+j*data_bytes]);
//                float *ptr = NULL;
//                ptr = &(float*)temp;
                acc[j].data1 = temp;
                //accel[j] = acc[j].data2;
                accel[j] = lpf(acc[j].data2, accel[j], 13.0f);
            }
        }
        if(buffer[7+len1+len2+1]== 0x80&&buffer[7+len1+len2+2]==0x20){
            len3 = buffer[7+len1+len2+3];
            data_bytes = len3/3;
            for(int j=0;j<3;j++){
                uint32_t temp = (buffer[11+len1+len2+j*data_bytes]<<24) | (buffer[12+len1+len2+j*data_bytes]<<16) | (buffer[13+len1+len2+j*data_bytes]<<8) | (buffer[14+len1+len2+j*data_bytes]);
//                float *ptr = NULL;
//                ptr = (float)temp;
                gry[j].data1 = temp;
                //omega[j] = gry[j].data2;
                omega[j] = lpf(gry[j].data2, omega[j], 13.0f);
            }
        }

    }
    // calculate
    accel_[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
    accel_[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
    accel_[2] = accel[2];
//    printf("NO ??? \r\n");
}

void MTi2_Init(){
    //LL_SPI_Enable(SPI3);//Enable SPI
//    printf("Init \r\n");
    cs_MTI = 1;///???
    ConfigureProt(1,0,0,0);//Configure DRDY

    //LL_mDelay(500);//Wait 500ms MTi2 go to measurement mode

}

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