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:
- 2016-09-20
- Revision:
- 24:cbdf0f7d33bd
- Parent:
- 23:f170a1d72a84
- Child:
- 25:6e43bbe76aec
File content as of revision 24:cbdf0f7d33bd:
#include "mbed.h" #include "FXOS8700Q.h" #include "FXAS21000.h" #include "MBed_Adafruit_GPS.h" #include "Hexi_OLED_SSD1351.h" #include "sensor_fusion.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 #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 FXOS8700Q_acc acc( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear FXOS8700Q_mag mag( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear MotionSensorDataUnits mag_data; MotionSensorDataUnits acc_data; MotionSensorDataCounts mag_raw; MotionSensorDataCounts acc_raw; 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; void gps_thread(void const *args) { 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); } } 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() { 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 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()); /* 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 Thread gps_t(gps_thread); Thread imu_t(imu_thread); 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*/ /* 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%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); } }