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-08-18
- Revision:
- 16:2e42284011d9
- Parent:
- 15:b5a3e22d706b
- Child:
- 17:d30e11e60dba
File content as of revision 16:2e42284011d9:
#include "mbed.h" #include "FXOS8700Q.h" #include "FXAS21000.h" #include "MBed_Adafruit_GPS.h" #include "Adafruit_SSD1351.h" //#include "NeatGUI.h" DigitalOut myled(LED1); DigitalOut BOOSTEN(PTC13); //oled power enable Serial gps(PTD3,PTD2); Serial pc(USBTX, USBRX); //SSD1351_SPI OLED96(PTB22,PTB23,PTB21,PTB20,PTD15); //Hexiwear pins for SPI bus OLED (MOSI, MISO, SCK, CS, DC) Adafruit_SSD1351 OLED96(PTB20,PTE6,PTD15,PTB21,PTB22); //Hexiwear pins for SPI bus OLED (CS, RS, DC, SCK, MOSI, MISO); //#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 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 myGPS.begin(115200); //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); pc.baud(9600); 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()); BOOSTEN = 1; OLED96.begin(); //OLED96.invert(true); OLED96.setRotation(0); OLED96.setTextColor(OLED96.Color565(0,255,0)); OLED96.setTextWrap(true); OLED96.on(); OLED96.fillScreen(BLACK); wait(0.5); OLED96.printf(" Hello from Hexi"); OLED96.drawPixel(0,0,GREEN); OLED96.drawPixel(64,64,RED); OLED96.drawPixel(0,64,BLUE); OLED96.drawPixel(64,0,WHITE); wait(0.5); /* OLED96.open(); OLED96.state(Display::DISPLAY_ON); OLED96.drawCircle(10,10,5,0xFFFFFFFF); OLED96.flush(); wait(2.0); */ //OLED128.state(Display::DISPLAY_OFF); //BOOSTEN = 0; while (true) { 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]); 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); 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); 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); /* while(gps.readable()) { c = myGPS.read(); //queries the GPS if (c) { pc.printf("%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; } } */ wait(0.1); } }