main.cpp

00001 #include "mbed.h"
00002 #include "FXOS8700CQ.h"
00003 #include "FXAS21000.h"
00004 #include "MBed_Adafruit_GPS.h"
00005 #include "Hexi_OLED_SSD1351.h"
00006 #include "MadgwickAHRS.h"
00007 #include "images.h"
00008 
00009 #include "rtos.h"
00010 
00011 
00012 DigitalOut myled(LED1);
00013 
00014 Serial gps(PTD3,PTD2);
00015 Serial camera(PTC17,PTC16);
00016 
00017 Adafruit_GPS myGPS(&gps);
00018 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
00019 
00020 #define DEBUG
00021 
00022 #ifdef DEBUG
00023 Serial pc(USBTX, USBRX);
00024 #define LOG(args...)    pc.printf(args)
00025 #else
00026 #define LOG(args...)
00027 #endif
00028 
00029 /* Instantiate the SSD1351 OLED Driver */
00030 SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */
00031 
00032 //#define BLACK 0x000000
00033 //#define WHITE 0xFFFFFF
00034 #define GREEN 0x00FF00
00035 #define RED 0xFF0000
00036 #define BLUE 0x0000FF
00037 
00038 FXOS8700CQ combo( PTC11, PTC10); // Proper Ports and I2C Address for Hexiwear
00039 
00040 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
00041 
00042 Data combo_data;
00043 float gyro_data[3];
00044 
00045 Timer t;
00046 
00047 //Worker threads
00048 Thread gps_t,imu_t;
00049 
00050 
00051 //GPS Reader worker task
00052 void gps_thread(void)
00053 {
00054     while (true) {
00055         c = myGPS.read();   //queries the GPS
00056         
00057         //check if we recieved a new message from GPS, if so, attempt to parse it,
00058         if ( myGPS.newNMEAreceived() ) {
00059             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00060                 LOG(myGPS.lastNMEA());
00061                 continue;
00062             }
00063         }
00064         Thread::wait(1);
00065     }
00066 }
00067 
00068 //IMU reader and sensor fusion worker task
00069 void imu_thread(void)
00070 {
00071     while(true) {
00072         gyro.ReadXYZ(gyro_data);
00073         combo_data = combo.get_values();
00074         Thread::wait(10);
00075     }
00076 }
00077 
00078 int main()
00079 {
00080     char text[30];  /* Text Buffer */
00081 
00082 //Start timer
00083     t.start();
00084 
00085     myGPS.begin(9600);
00086 //Turn off all sentences except GGA and RMC
00087 //For MTK GPS
00088     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
00089 
00090 //FOR UBLOX GPS
00091     myGPS.sendCommand(UBX_DISABLE_ZDA);
00092     myGPS.sendCommand(UBX_DISABLE_GLL);
00093     myGPS.sendCommand(UBX_DISABLE_VTG);
00094     myGPS.sendCommand(UBX_DISABLE_GSV);
00095     myGPS.sendCommand(UBX_DISABLE_GSA);
00096 
00097 
00098 #ifdef DEBUG
00099     pc.baud(115200);
00100 #endif
00101 
00102 //Connect to Nikon D800 UART at 4800baud
00103     camera.baud(4800);
00104 
00105 //Initialise gyro
00106     char whoami = gyro.getWhoAmI();
00107 //Initialise combo (acc&mag)
00108     combo.init();
00109 
00110     /* Get OLED Class Default Text Properties */
00111     oled_text_properties_t textProperties = {0};
00112     oled.GetTextProperties(&textProperties);
00113 
00114     /* Turn on the backlight of the OLED Display */
00115     oled.DimScreenON();
00116 
00117     /* Fills the screen with solid black */
00118     oled.FillScreen(COLOR_BLACK);
00119 
00120     /* Adding full screen splash */
00121     const uint8_t *image = NXP_whole_bmp;
00122     /* Draws the image on the Screen starting at (x=0,y=0) */
00123     oled.DrawImage(image,0,0);
00124 
00125     Thread::wait(1000);
00126 
00127     /* Fills the screen with solid black */
00128     oled.FillScreen(COLOR_BLACK);
00129 
00130     /* Display Text at (x=7,y=0) */
00131     strcpy((char *) text,"AERO GPS-AHRS");
00132     oled.Label((uint8_t *)text,7,0);
00133 
00134     /* Change font color to blue */
00135     textProperties.fontColor   = COLOR_BLUE;
00136     oled.SetTextProperties(&textProperties);
00137 
00138 
00139     /* Display text at (x=1,y=15) MAG label*/
00140     strcpy(text,"M:");
00141     oled.Label((uint8_t *)text,1,15);
00142 
00143     /* Display text at (x=1,y=30) ACC label*/
00144     strcpy(text,"A:");
00145     oled.Label((uint8_t *)text,1,30);
00146 
00147     /* Display text at (x=1,y=45) GYRO label*/
00148     strcpy(text,"G:");
00149     oled.Label((uint8_t *)text,1,45);
00150 
00151     /* Display text at (x=1,y=60) Time label*/
00152     strcpy(text,"Timer(s):");
00153     oled.Label((uint8_t *)text,1,60);
00154 
00155     /* Display text at (x=1,y=75) Position label*/
00156     strcpy(text,"P:");
00157     oled.Label((uint8_t *)text,1,75);
00158 
00159     /* Set text properties to white and right aligned for the dynamic text */
00160     textProperties.fontColor = COLOR_WHITE;
00161     textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
00162     oled.SetTextProperties(&textProperties);
00163 
00164     //Start read and update threads
00165     gps_t.start(gps_thread);
00166     imu_t.start(imu_thread);
00167 
00168     while (true) {     
00169         /* Format the GYRO reading */
00170         sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
00171         /* Display gyro to 2 decimal */
00172         oled.TextBox((uint8_t *)text,15,45,80,15); /*Expand textbox for more digits*/
00173 
00174 
00175         /* Format the time reading */
00176         sprintf(text,"%.2f",t.read());
00177 
00178         /* Display time reading */
00179         oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/
00180 
00181         if(myGPS.fix) {
00182             LOG(myGPS.lastNMEA());
00183             sprintf(text,"%.2f%c %.2f%c",myGPS.longitude/100.0f,myGPS.lat,myGPS.latitude/100.0f,myGPS.lon);
00184         } else {
00185             sprintf(text,"No GPS");
00186         }
00187 
00188         /* Display GPS location or lock status */
00189         oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
00190         Thread::wait(1000);
00191     }
00192 }