File content as of revision 27:8c84611a4ca5:

#include "mbed.h"
#include "FXOS8700CQ.h"
#include "FXAS21000.h"
#include "MBed_Adafruit_GPS.h"
#include "Hexi_OLED_SSD1351.h"
#include "MadgwickAHRS.h"
#include "images.h"

#include "rtos.h"


DigitalOut myled(LED1);

Serial gps(PTD3,PTD2);
Serial camera(PTC17,PTC16);

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

FXOS8700CQ combo( PTC11, PTC10); // Proper Ports and I2C Address for Hexiwear

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

Data combo_data;
float gyro_data[3];

Timer t;

//Worker threads
Thread gps_t,imu_t;


//GPS Reader worker task
void gps_thread(void)
{
    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);
    }
}

//IMU reader and sensor fusion worker task
void imu_thread(void)
{
    while(true) {
        gyro.ReadXYZ(gyro_data);
        combo_data = combo.get_values();
        Thread::wait(10);
    }
}

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

//Connect to Nikon D800 UART at 4800baud
    camera.baud(4800);

//Initialise gyro
    char whoami = gyro.getWhoAmI();
//Initialise combo (acc&mag)
    combo.init();

    /* 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
    gps_t.start(gps_thread);
    imu_t.start(imu_thread);

    while (true) {     
        /* 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);
    }
}