Use hexiwear as a GPSIMU-AHRS for Nikon DSLR cameras
Dependencies: FXOS8700CQ FXAS21000 MBed_Adafruit-GPS-Library Hexi_OLED_SSD1351 Hexi_KW40Z Madgwick
Fork of Hexi_Blinky_Example by
Diff: main.cpp
- Revision:
- 23:f170a1d72a84
- Parent:
- 22:e69bc54ca4c0
- Child:
- 24:cbdf0f7d33bd
diff -r e69bc54ca4c0 -r f170a1d72a84 main.cpp --- a/main.cpp Mon Sep 19 04:59:45 2016 +0000 +++ b/main.cpp Mon Sep 19 16:27:18 2016 +0000 @@ -3,12 +3,16 @@ #include "FXAS21000.h" #include "MBed_Adafruit_GPS.h" #include "Hexi_OLED_SSD1351.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 +#define DEBUG #ifdef DEBUG Serial pc(USBTX, USBRX); @@ -36,19 +40,75 @@ 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; int lastUpdate; +void gps_thread(void const *args) +{ + while (true) { + c = myGPS.read(); //queries the GPS + if (c) { + LOG("%c", c); //this line will echo the GPS data if not paused + } + //check if we recieved a new message from GPS, if so, attempt to parse it, + if ( myGPS.newNMEAreceived() ) { + if ( !myGPS.parse(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() { - 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 @@ -71,6 +131,9 @@ pc.baud(115200); #endif + Thread gps_t(gps_thread); + Thread imu_t(imu_thread); + 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()); @@ -85,6 +148,16 @@ /* 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); @@ -120,81 +193,38 @@ oled.SetTextProperties(&textProperties); - while (true) { + 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*/ - c = myGPS.read(); //queries the GPS + /* 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*/ - //check if we recieved a new message from GPS, if so, attempt to parse it, - if ( myGPS.newNMEAreceived() ) { - if ( !myGPS.parse(myGPS.lastNMEA()) ) { - continue; - } + /* 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"); } - int Now = t.read_ms(); - - if(Now - lastUpdate > 500) { - - 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); - - /* 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 %.2f %.2f",myGPS.longitude,myGPS.longitude,myGPS.altitude); - } 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*/ - - lastUpdate = t.read_ms(); - } + /* Display GPS location or lock status */ + oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/ + Thread::wait(1000); } }