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
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 }
Generated on Wed Jul 13 2022 10:06:28 by 1.7.2