File content as of revision 24:cbdf0f7d33bd:

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

#include "rtos.h"


DigitalOut myled(LED1);

Serial gps(PTD3,PTD2);
Adafruit_GPS myGPS(&gps);
char c; //when read via Adafruit_GPS::read(), the class returns single character stored here

#define DEBUG

#ifdef DEBUG
Serial pc(USBTX, USBRX);
#define LOG(args...)    pc.printf(args)
#else
#define LOG(args...)
#endif

/* 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

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

Timer t;

void gps_thread(void const *args)
{
    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()) ) {
                LOG(myGPS.lastNMEA());
                continue;
            }
        }
        Thread::wait(1);
    }
}

void imu_thread(void const *args)
{
    while(true) {
        acc.getAxis(acc_data);
        mag.getAxis(mag_data);
        gyro.ReadXYZ(gyro_data);
        
        LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", acc_data.x, acc_data.y, acc_data.z);
        LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
        LOG("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);
        
        LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", faX, faY, faZ);
        LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
        
        acc.getAxis(acc_raw);
        mag.getAxis(mag_raw);
        
        LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", acc_raw.x, acc_raw.y, acc_raw.z);
        LOG("    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);
        
        LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", raX, raY, raZ);
        LOG("    MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
        
        Thread::wait(100);
    }
}

int main()
{
    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);


#ifdef DEBUG
    pc.baud(115200);
#endif

    acc.enable();
    LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
    LOG("\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);

    /* Adding full screen splash */
    const uint8_t *image = NXP_whole_bmp;
    /* Draws the image on the Screen starting at (x=0,y=0) */
    oled.DrawImage(image,0,0);

    Thread::wait(1000);

    /* 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=15) MAG label*/
    strcpy(text,"M:");
    oled.Label((uint8_t *)text,1,15);

    /* Display text at (x=1,y=30) ACC label*/
    strcpy(text,"A:");
    oled.Label((uint8_t *)text,1,30);

    /* Display text at (x=1,y=45) GYRO label*/
    strcpy(text,"G:");
    oled.Label((uint8_t *)text,1,45);

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

    /* Display text at (x=1,y=75) Position label*/
    strcpy(text,"P:");
    oled.Label((uint8_t *)text,1,75);

    /* 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);

    //Start read and update threads
    Thread gps_t(gps_thread);
    Thread imu_t(imu_thread);

    while (true) {     
        /* Format the MAG reading */
        sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
        /* Display Mag readings to 1 decimal */
        oled.TextBox((uint8_t *)text,15,15,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,30,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,45,80,15); /*Expand textbox for more digits*/


        /* Format the time reading */
        sprintf(text,"%.2f",t.read());

        /* Display time reading */
        oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/

        if(myGPS.fix) {
            LOG(myGPS.lastNMEA());
            sprintf(text,"%.2f%c %.2f%c",myGPS.longitude/100.0f,myGPS.lat,myGPS.latitude/100.0f,myGPS.lon);
        } else {
            sprintf(text,"No GPS");
        }

        /* Display GPS location or lock status */
        oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
        Thread::wait(1000);
    }
}