File content as of revision 14:c357be9a3fc8:

#include "AHRS.h"
#include "Mahony.h"
#include "MadgwickAHRS.h"
#include "ekf.h"

#define PI 3.141592653589793

using namespace std;

//OLD: AHRS::AHRS(uint8_t filtertype,float TS) : RPY_filter(TS), csAG(PA_15),csM(PD_2), spi(PC_12, PC_11, PC_10), imu(&spi, &csM, &csAG), thread(osPriorityBelowNormal, 4096){  
AHRS::AHRS(uint8_t filtertype,float TS,bool calib) :  imu(PC_9, PA_8, 0xD6, 0x3C), RPY_filter(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096){  
    float g[3];
    g[0]=0.0;g[1]=0.0;g[2]=0.0;   
    float a[3];
    a[0]=0.0;a[1]=0.0;a[2]=0.0; 
    uint8_t N = 100;  
    while (!imu.begin()) {
        wait(1);
        printf("Failed to communicate with LSM9DS1.\r\n");
    }
    if(calib){
        wait_ms(500);
        for(int k=0;k < N;k++)
            {
                imu.readGyro();
                imu.readAccel();
                g[0]+= imu.gyroX;
                g[1]+= imu.gyroY;
                g[2]+= imu.gyroZ;
                a[0]+= imu.accX;
                a[1]+= imu.accY;
                a[2]+= imu.accZ;
                wait_ms(10);
                }
        for(int k = 0;k<3;k++){
            g[k] /= (float)N;
            a[k] /= (float)N;
            }
        printf("Correct g: %1.5f %1.5f %1.5f a: %1.5f %1.5f %1.5f\r\n",g[0],g[1],g[2],a[0],a[1],a[2]); 
        }
    raw_gx2gx.setup( 1.0,g[0]);
    raw_gy2gy.setup(-1.0,g[1]);  // y-axis reversed (lefthanded system)
    raw_gz2gz.setup( 1.0,g[2]);
    raw_ax2ax.setup( 1.0,a[0]);       // use gain and offset here
    raw_ay2ay.setup(-1.0,a[1]);      // y-axis reversed // was -1,0.0
    raw_az2az.setup( 1.0,0.0);
    int2magx.setup(  -1.0,0.0);     // x-axis reversed
    int2magy.setup(  -1.0,0.0);     // y-axis reversed
    int2magz.setup(  1.0,0.0);
    local_time = 0.0;
// The Thread starts
    thread.start(callback(this, &AHRS::update));
    ticker.attach(callback(this, &AHRS::sendSignal), TS);

    

}

AHRS::~AHRS() {}


void AHRS::update(void)
{
    while(1){   
        thread.signal_wait(signal);
        imu.readAccel();
        //imu.readMag_calibrated();
        imu.readMag();
        imu.readGyro();
        matrix measurement(6,1,0.0);
        //Perform filter update
        //Mahony_filter.update(raw_gx2gx(imu.gyroX), raw_gy2gy(imu.gyroY),  raw_gz2gz(imu.gyroZ) ,
        //              raw_ax2ax(imu.accX), raw_ay2ay(imu.accY),  raw_az2az(imu.accZ),
        //              int2magx(imu.magX), int2magy(imu.magY),  int2magz(imu.magZ));
        measurement.put_entry(0,0,raw_gx2gx(imu.gyroX));
        measurement.put_entry(1,0,raw_gy2gy(imu.gyroY));
        measurement.put_entry(3,0,raw_ax2ax(imu.accX));
        measurement.put_entry(4,0,raw_ay2ay(imu.accY));
        RPY_filter.loop(&measurement);

        my_logger.data_vector[0] = raw_gx2gx(imu.gyroX);
        my_logger.data_vector[1] = raw_gy2gy(imu.gyroY);
        my_logger.data_vector[2] = raw_gz2gz(imu.gyroZ);
        my_logger.data_vector[3] = raw_ax2ax(imu.accX);
        my_logger.data_vector[4] = raw_ay2ay(imu.accY);
        my_logger.data_vector[5] = raw_az2az(imu.accZ);
        my_logger.data_vector[6] = RPY_filter.get_est_state(0);
        my_logger.data_vector[7] = RPY_filter.get_est_state(1);
        my_logger.data_vector[8] = RPY_filter.get_est_state(4);
        my_logger.data_vector[9] = RPY_filter.get_est_state(5);
        }       // while(1) (the thread)
        
}
void AHRS::sendSignal() {
    
    thread.signal_set(signal);
}