File content as of revision 21:b165e847c5ba:

#include "mbed.h"
#include "FXOS8700Q.h"
#include "FXAS21000.h"
#include "MBed_Adafruit_GPS.h"
#include "Hexi_OLED_SSD1351.h"

DigitalOut myled(LED1);

Serial gps(PTD3,PTD2);
Serial pc(USBTX, USBRX);

/* Instantiate the SSD1351 OLED Driver */
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */

//#define BLACK 0x000000
//#define WHITE 0xFFFFFF
#define GREEN 0x00FF00
#define RED 0xFF0000
#define BLUE 0x0000FF

FXOS8700Q_acc acc( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
FXOS8700Q_mag mag( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
MotionSensorDataUnits mag_data;
MotionSensorDataUnits acc_data;

MotionSensorDataCounts mag_raw;
MotionSensorDataCounts acc_raw;

FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear

Timer t;
int lastUpdate;

int main()
{
    float faX, faY, faZ;
    float fmX, fmY, fmZ;
    int16_t raX, raY, raZ;
    int16_t rmX, rmY, rmZ;
    float gyro_data[3];

    Adafruit_GPS myGPS(&gps);
    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
    char text[30];  /* Text Buffer */ 

//Start timer
    t.start();

    myGPS.begin(9600);
//Turn off all sentences except GGA and RMC
//For MTK GPS
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);

//FOR UBLOX GPS
    myGPS.sendCommand(UBX_DISABLE_ZDA);
    myGPS.sendCommand(UBX_DISABLE_GLL);
    myGPS.sendCommand(UBX_DISABLE_VTG);
    myGPS.sendCommand(UBX_DISABLE_GSV);
    myGPS.sendCommand(UBX_DISABLE_GSA);


    pc.baud(115200);

    acc.enable();
    pc.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
    pc.printf("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI());

    /* Get OLED Class Default Text Properties */
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);

    /* Turn on the backlight of the OLED Display */
    oled.DimScreenON();

    /* Fills the screen with solid black */
    oled.FillScreen(COLOR_BLACK);

    /* Display Text at (x=7,y=0) */
    strcpy((char *) text,"AERO GPS-AHRS");
    oled.Label((uint8_t *)text,7,0);

    /* Change font color to blue */
    textProperties.fontColor   = COLOR_BLUE;
    oled.SetTextProperties(&textProperties);
    
    
    /* Display text at (x=1,y=10) MAG label*/
    strcpy(text,"M:");
    oled.Label((uint8_t *)text,1,10);
    
    /* Display text at (x=1,y=25) ACC label*/
    strcpy(text,"A:");
    oled.Label((uint8_t *)text,1,25);
    
    /* Display text at (x=1,y=40) GYRO label*/
    strcpy(text,"G:");
    oled.Label((uint8_t *)text,1,40);

    /* Display text at (x=1,y=55) Time label*/
    strcpy(text,"Timer(s):");
    oled.Label((uint8_t *)text,1,55);
    
    /* Display text at (x=1,y=70) Position label*/
    strcpy(text,"P:");
    oled.Label((uint8_t *)text,1,70);

    /* Set text properties to white and right aligned for the dynamic text */
    textProperties.fontColor = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
    oled.SetTextProperties(&textProperties);


    while (true) {

        c = myGPS.read();   //queries the GPS

        //check if we recieved a new message from GPS, if so, attempt to parse it,
        if ( myGPS.newNMEAreceived() ) {
            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
                continue;
            }
        }

        int Now = t.read_ms();

        if(Now - lastUpdate > 500) {

            acc.getAxis(acc_data);
            mag.getAxis(mag_data);
            gyro.ReadXYZ(gyro_data);
            pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", acc_data.x, acc_data.y, acc_data.z);
            pc.printf("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
            pc.printf("FXAS21000      X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
            acc.getX(&faX);
            acc.getY(&faY);
            acc.getZ(&faZ);
            mag.getX(&fmX);
            mag.getY(&fmY);
            mag.getZ(&fmZ);
            pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", faX, faY, faZ);
            pc.printf("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
            acc.getAxis(acc_raw);
            mag.getAxis(mag_raw);
            pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", acc_raw.x, acc_raw.y, acc_raw.z);
            pc.printf("    MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
            acc.getX(&raX);
            acc.getY(&raY);
            acc.getZ(&raZ);
            mag.getX(&rmX);
            mag.getY(&rmY);
            mag.getZ(&rmZ);
            pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", raX, raY, raZ);
            pc.printf("    MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
            
            /* Format the MAG reading */
            sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
            /* Display Mag readings to 1 decimal */
            oled.TextBox((uint8_t *)text,15,10,80,15); /*Expand textbox for more digits*/ 
            
            /* Format the ACC reading */
            sprintf(text,"%.2f %.2f %.2f",faX,faY,faZ);
            /* Display acceleration to 2 decimal */
            oled.TextBox((uint8_t *)text,15,25,80,15); /*Expand textbox for more digits*/ 
            
            /* Format the GYRO reading */
            sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
            /* Display gyro to 2 decimal */
            oled.TextBox((uint8_t *)text,15,40,80,15); /*Expand textbox for more digits*/ 
            
            
            /* Format the time reading */
            sprintf(text,"%.2f",t.read());
            
            /* Display time reading in 35px by 15px textbox at(x=55, y=55) */
            oled.TextBox((uint8_t *)text,55,55,35,15); /*Expand textbox for more digits*/ 
            
            if(myGPS.fix) {
                pc.printf(myGPS.lastNMEA());
                sprintf(text,"%.2f %.2f %.2f",myGPS.longitude,myGPS.longitude,myGPS.altitude);
                /* Display GPS location to 2 decimal */
                oled.TextBox((uint8_t *)text,15,70,80,15); /*Expand textbox for more digits*/ 
            }
            else
            {
                sprintf(text,"No GPS");
                /* Display lock status */
                oled.TextBox((uint8_t *)text,15,70,80,15); /*Expand textbox for more digits*/ 
            }
            
            
            lastUpdate = t.read_ms();
        }
    }
}