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:
- 22:e69bc54ca4c0
- Parent:
- 21:b165e847c5ba
- Child:
- 23:f170a1d72a84
--- a/main.cpp Mon Sep 19 04:34:11 2016 +0000 +++ b/main.cpp Mon Sep 19 04:59:45 2016 +0000 @@ -7,7 +7,15 @@ DigitalOut myled(LED1); Serial gps(PTD3,PTD2); + +//#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) */ @@ -41,7 +49,7 @@ Adafruit_GPS myGPS(&gps); char c; //when read via Adafruit_GPS::read(), the class returns single character stored here - char text[30]; /* Text Buffer */ + char text[30]; /* Text Buffer */ //Start timer t.start(); @@ -59,11 +67,13 @@ myGPS.sendCommand(UBX_DISABLE_GSA); +#ifdef DEBUG pc.baud(115200); +#endif acc.enable(); - pc.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI()); - pc.printf("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI()); + LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI()); + LOG("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI()); /* Get OLED Class Default Text Properties */ oled_text_properties_t textProperties = {0}; @@ -82,27 +92,27 @@ /* Change font color to blue */ textProperties.fontColor = COLOR_BLUE; oled.SetTextProperties(&textProperties); - - - /* Display text at (x=1,y=10) MAG label*/ + + + /* Display text at (x=1,y=15) MAG label*/ strcpy(text,"M:"); - oled.Label((uint8_t *)text,1,10); - - /* Display text at (x=1,y=25) ACC label*/ + 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,25); - - /* Display text at (x=1,y=40) GYRO label*/ + 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,40); + oled.Label((uint8_t *)text,1,45); - /* Display text at (x=1,y=55) Time label*/ + /* Display text at (x=1,y=60) Time label*/ strcpy(text,"Timer(s):"); - oled.Label((uint8_t *)text,1,55); - - /* Display text at (x=1,y=70) Position label*/ + 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,70); + oled.Label((uint8_t *)text,1,75); /* Set text properties to white and right aligned for the dynamic text */ textProperties.fontColor = COLOR_WHITE; @@ -128,66 +138,62 @@ acc.getAxis(acc_data); mag.getAxis(mag_data); gyro.ReadXYZ(gyro_data); - pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", acc_data.x, acc_data.y, acc_data.z); - pc.printf(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z); - pc.printf("FXAS21000 X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]); + 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); - pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", faX, faY, faZ); - pc.printf(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, 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); - pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d ", acc_raw.x, acc_raw.y, acc_raw.z); - pc.printf(" MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z); + 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); - pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d ", raX, raY, raZ); - pc.printf(" MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, 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,10,80,15); /*Expand textbox for more digits*/ - + 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,25,80,15); /*Expand textbox for more digits*/ - + 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,40,80,15); /*Expand textbox for more digits*/ - - + 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 in 35px by 15px textbox at(x=55, y=55) */ - oled.TextBox((uint8_t *)text,55,55,35,15); /*Expand textbox for more digits*/ - + + /* Display time reading */ + oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/ + if(myGPS.fix) { - pc.printf(myGPS.lastNMEA()); + LOG(myGPS.lastNMEA()); sprintf(text,"%.2f %.2f %.2f",myGPS.longitude,myGPS.longitude,myGPS.altitude); - /* Display GPS location to 2 decimal */ - oled.TextBox((uint8_t *)text,15,70,80,15); /*Expand textbox for more digits*/ - } - else - { + } else { sprintf(text,"No GPS"); - /* Display lock status */ - oled.TextBox((uint8_t *)text,15,70,80,15); /*Expand textbox for more digits*/ } - + /* Display GPS location or lock status */ + oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/ + lastUpdate = t.read_ms(); } }