main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "MadgwickAHRS.h"
00004 
00005 RawSerial pc(USBTX, USBRX);
00006 
00007 LSM9DS1 lsm = LSM9DS1(PB_5, PB_4, PB_3,PB_10,PA_8); //mosi,miso,sclk,cs_AG,cs_M
00008 Ticker AHRS;
00009 Ticker PRINT;
00010 DigitalOut myled(LED1);
00011 Madgwick filter = Madgwick();
00012 
00013 void ahrs(){
00014     filter.update(lsm.gyro_y, lsm.gyro_x, lsm.gyro_z, lsm.accel_y, lsm.accel_x, lsm.accel_z ,lsm.mag_y, lsm.mag_x, lsm.mag_z);  
00015 }
00016 
00017 void print(){
00018     printf("roll:%f,pitch:%f,yaw:%f,accel_z:%f,gyro_z:%f,mag_x:%f\r\n", filter.getRoll(),filter.getPitch(),filter.getYaw(),lsm.accel_z, lsm.gyro_z,lsm.mag_x);
00019 }
00020 
00021 int main() {
00022     lsm.initSPI();
00023     lsm.initAccel(lsm.LSM9DS1_ACCELRANGE_8G);   //2,4,8,16
00024     lsm.initGyro(lsm.LSM9DS1_GYROSCALE_500DPS); //245,500,2000
00025     lsm.initMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);    //4,8,12,16
00026 
00027     filter.begin(100); // N Hz
00028     AHRS.attach_us(&ahrs, 10000); // N us周期
00029 
00030     PRINT.attach(&print,1);
00031 
00032     while(1){
00033         lsm.readGyro();
00034         lsm.readAccel();
00035         lsm.readMag();
00036         wait(0.1);
00037     }
00038 }