AHRS.cpp

00001 #include "AHRS.h"
00002 #include "Mahony.h"
00003 #include "MadgwickAHRS.h"
00004 #define PI 3.141592653589793
00005 
00006 using namespace std;
00007 
00008 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){  
00009     
00010     thread.start(callback(this, &AHRS::update));
00011     ticker.attach(callback(this, &AHRS::sendSignal), TS);
00012     LinearCharacteristics raw_ax2ax(40.0/65536.0,-418.0);       // use gain and offset here
00013     LinearCharacteristics raw_ay2ay(-40.0/65536.0,-307.0);      // y-axis reversed
00014     LinearCharacteristics raw_az2az(-16350.0,16350,-10.0, 10.0);
00015     LinearCharacteristics raw_gx2gx(-32767,32768,-500*PI/180.0, 500*PI/180.0);
00016     LinearCharacteristics raw_gy2gy(-32767,32768, 500*PI/180.0,-500*PI/180.0);  // y-axis reversed (lefthanded system)
00017     LinearCharacteristics raw_gz2gz(-32767,32768,-500*PI/180.0, 500*PI/180.0);
00018     LinearCharacteristics int2magx( -32767,32768,100.0,-100.0);     // x-axis reversed
00019     LinearCharacteristics int2magy( -32767,32768,100.0,-100.0);     // y-axis reversed
00020     LinearCharacteristics int2magz( -32767,32768,-100.0,100.0);
00021     while (!imu.begin()) {
00022         wait(1);
00023         printf("Failed to communicate with LSM9DS1.\r\n");
00024     }
00025 
00026 }
00027 
00028 AHRS::~AHRS() {}
00029 
00030 void AHRS::update(void)
00031 {
00032     while(1){
00033         thread.signal_wait(signal);
00034         imu.readAccel();
00035         imu.readMag_calibrated();
00036         imu.readGyro();
00037 
00038         //Perform Madgwick-filter update
00039         RPY_filter.update(raw_gx2gx(imu.gx), raw_gy2gy(imu.gy),  raw_gz2gz(imu.gz) ,
00040                       raw_ax2ax(imu.ax), raw_ay2ay(imu.ay),  raw_az2az(imu.az),
00041                       int2magx(imu.mx), int2magy(imu.my),  int2magz(imu.mz)); 
00042        
00043  
00044         }       // while(1)
00045 }
00046 void AHRS::sendSignal() {
00047     
00048     thread.signal_set(signal);
00049 }