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
main.cpp
- Committer:
- whatnick
- Date:
- 2017-03-08
- Revision:
- 27:8c84611a4ca5
- Parent:
- 26:e35dd673fd13
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); } }