File content as of revision 2:d068248795d9:

#include "mbed.h"
#include "BMX055.hpp"
#include "MadgwickFilter.hpp"
#define M_PI 3.14159265358979323846
#include "Quaternion.hpp"
#include "SphereFitting.hpp"

Serial pc(USBTX, USBRX);
DigitalOut led(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

// Read temperature from LM75BD
union Bits32 {
    signed int data_i32;
    unsigned int data_ui32;
    signed short data_i16;
    unsigned short data_ui16;
    float data_f32;
    char bytes[4];
};

static const int NUMOFCH = 1;//4;// 1; //9;
//BMX055 imu(0x19, 0x69, 0x13, p9,  p10);
BMX055 imu(0x19, 0x69, 0x13, p28,  p27);

DigitalOut imu_sel[9] = {
    DigitalOut(p11), DigitalOut(p12), DigitalOut(p13),
    DigitalOut(p14), DigitalOut(p15), DigitalOut(p16),
    DigitalOut(p17), DigitalOut(p18), DigitalOut(p19)
};

short offset_gyr[NUMOFCH][3];
int offset_mag[NUMOFCH][3];

MadgwickFilter mf[9];
/* = {
    MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f),
    MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f),
    MadgwickFilter(0.1f),MadgwickFilter(0.1f),MadgwickFilter(0.1f)
};
*/

const int bufsizeofLPF = 1;
Quaternion q_buf[NUMOFCH][bufsizeofLPF];  //  Buffers for low pass filter
Quaternion q_sum[NUMOFCH];            //  Sum of values in the buffers

int flag = 0;
const float dpspLSB = 0.0153; //0.0038;//(deg/s)/bit
const float rpspLSB = dpspLSB * M_PI / 180.0; //(rad/s)/bit
const float vpspLSB = 0.00098; // (m/s^2)/bit

//  For frequent loop
Ticker tick;
Timer timer;

void init();
void calib();
void update();
void senddata();
void initRawdata();
void sendRawdata();

int main()
{

    pc.baud(921600);
    pc.attach(&senddata);
    init();
    //initRawdata();
    wait_ms(300);
    tick.attach_us(update,5000);
    led2 = 1;
    //tick.attach(updateRawdata,0.1f);
    timer.start();

    while (1) {
        led = !led;
        wait(0.1);
    }
}


void init()
{
    led = led2 = led3 = led4 = 0;
    for (int i = 0; i < NUMOFCH; i++) {
        mf[i] = MadgwickFilter(0.1f);
    }

    for (int j = 0; j < NUMOFCH; j++) {
        for (int i = 0; i < bufsizeofLPF; i++) {
            q_buf[j][i].Set<float>(0,0,0,0);
        }
        for (int i = 0; i < 3; i ++) {
            offset_gyr[j][i] = 0;
            offset_mag[j][i] = 0;
        }
        q_sum[j].Set<float>(0,0,0,0);
        imu_sel[j] = 0;
    }
    led4 = 1;

    calib();
    led3 = 1;
}

void calib()
{
    // Calibration for offset of GYR
    for (int j = 0; j < NUMOFCH; j++) {
        imu_sel[j] = 1;
        imu_sel[j] = 1;
        for (int i = 0; i < 1000; i++) {
            led3 = !led3;
            imu.Update();
            offset_gyr[j][0] += imu.gyr.x;
            offset_gyr[j][1] += imu.gyr.y;
            offset_gyr[j][2] += imu.gyr.z;
            wait_ms(1);
        }
        imu_sel[j] = 0;
        imu_sel[j] = 0;
        offset_gyr[j][0] /= 1000;
        offset_gyr[j][1] /= 1000;
        offset_gyr[j][2] /= 1000;
    }
    led3 = 1;
    wait_ms(500);
    led3 = 0;
    wait_ms(500);
    // Calibration for offset of MAG
    for (int j = 0; j < NUMOFCH; j++) {
        SphereFitting<int> sphere(offset_mag[j][0], offset_mag[j][1], offset_mag[j][2]);
        imu_sel[j] = 1;
        imu_sel[j] = 1;
        for (int i = 0; i < 10000; i++) {
            led3 = !led3;
            imu.Update();
            sphere.addsample(imu.mag.x, imu.mag.y, imu.mag.z);
            wait_ms(1);
        }
        imu_sel[j] = 0;
        imu_sel[j] = 0;
        sphere.solve();
        sphere.copy_param_to(offset_mag[j], offset_mag[j]+1, offset_mag[j]+2, NULL);
    }
    
    led3 = 1;
}


void update()
{
    static int t = 0;
    /*
    static int t_now = 0, t_prev = 0;
    t_prev = timer.read_us();
    */
    for (int j = 0; j < NUMOFCH; j++) {
        imu_sel[j] = 1;
        imu_sel[j] = 1;
        //imu.UpdateIMU();
        imu.Update();
        imu_sel[j] = 0;
        imu_sel[j] = 0;
        /*
        mf[j].MadgwickAHRSupdateIMU(
            (imu.gyr.x - offset_gyr[j][0]) * rpspLSB,
            (imu.gyr.y - offset_gyr[j][1]) * rpspLSB,
            (imu.gyr.z - offset_gyr[j][2]) * rpspLSB,
            imu.acc.x, imu.acc.y, imu.acc.z);
            */
        mf[j].MadgwickAHRSupdate(
            (imu.gyr.x - offset_gyr[j][0]) * rpspLSB,
            (imu.gyr.y - offset_gyr[j][1]) * rpspLSB,
            (imu.gyr.z - offset_gyr[j][2]) * rpspLSB,
            imu.acc.x, imu.acc.y, imu.acc.z,
            -(imu.mag.y - offset_mag[j][1]), 
            imu.mag.x - offset_mag[j][0],
            imu.mag.z - offset_mag[j][2]);
        q_sum[j] -= q_buf[j][t];
        mf[j].getAttitude(&(q_buf[j][t]));
        q_sum[j] += q_buf[j][t];

        /*
        pc.printf("q_buf: %+f,%+f,%+f,%+f\t",
                  q_buf[j][t].w, q_buf[j][t].x, q_buf[j][t].y, q_buf[j][t].z);
        */
        /*
        pc.printf("gyr: %+f,%+f,%+f\t",
                  //imu.acc.x * vpspLSB, imu.acc.y * vpspLSB, imu.acc.z * vpspLSB);

                  (imu.gyr.x - offset_gyr[j][0]) * rpspLSB,
                  (imu.gyr.y - offset_gyr[j][1]) * rpspLSB,
                  (imu.gyr.z - offset_gyr[j][2]) * rpspLSB
                 );
        */
    }

    //puts("\n");

    t = (t + 1) % bufsizeofLPF;
    if(t == 0) flag = 1;
    /*
    t_now = timer.read_us();
    pc.printf("updating duration: %d[us]\n",t_now - t_prev);
    */
}

void senddata()
{
    static Bits32 q_bits[4];
    static Bits32 tt;

    int c = pc.getc();

    if(c == 0xee) {
        tick.detach();
        init();
        tick.attach_us(update,5000);
    }

    if(c != 0xff || flag == 0) return;

    tt.data_ui32 = timer.read_us();
    for(int j = 0; j < 4; j++) {
        pc.putc(tt.bytes[j]);
    }

    for(int k = 0; k < NUMOFCH; k++) {
        q_bits[0].data_f32 = q_sum[k].w / (float)bufsizeofLPF;
        q_bits[1].data_f32 = q_sum[k].x / (float)bufsizeofLPF;
        q_bits[2].data_f32 = q_sum[k].y / (float)bufsizeofLPF;
        q_bits[3].data_f32 = q_sum[k].z / (float)bufsizeofLPF;
        for(int j = 0; j < 4; j++) {
            for(int i = 0; i < 4; i++) {
                pc.putc(q_bits[j].bytes[i]);
            }
        }
    }
}

void initRawdata()
{
    init();

    while(1) {
        char c = pc.getc();
        if (c == 0xff) {
            for(int i = 0; i < 4; i++) {
                pc.putc(0xff);
            }
            pc.puts("gyr-x,gyr-y,gyr-z,acc-x,acc-y,acc-z,mag-x,mag-y,mag-z,\n");
            break;
        }
    }
}



void sendRawdata()
{
    static Bits32 tmp;

    int c = pc.getc();

    if(c != 0xf0) return;

    for (int k = 0; k < NUMOFCH; k++) {
        imu_sel[k] = 1;
        imu.Update();
        imu_sel[k] = 0;

        for(int j = 0; j < 3; j++) {
            tmp.data_i16 = imu.gyr.e[j];
            for(int i = 0; i < 2; i++) {
                pc.putc(tmp.bytes[i]);
            }
        }

        for(int j = 0; j < 3; j++) {
            tmp.data_i16 = imu.acc.e[j];
            for(int i = 0; i < 2; i++) {
                pc.putc(tmp.bytes[i]);
            }
        }

        for(int j = 0; j < 3; j++) {
            tmp.data_i16 = imu.mag.e[j];
            for(int i = 0; i < 2; i++) {
                pc.putc(tmp.bytes[i]);
            }
        }
    }

}