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 Hexiwear

/media/uploads/whatnick/hexiwear_docking_station_numbers.jpg

Committer:
whatnick
Date:
Thu Aug 18 23:29:27 2016 +0000
Revision:
18:76f4fc030036
Parent:
17:d30e11e60dba
Child:
20:5a4e47822d79
Offsets to work with Hexi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan 0:7dec7e9ac085 1 #include "mbed.h"
whatnick 14:9885c8536437 2 #include "FXOS8700Q.h"
whatnick 15:b5a3e22d706b 3 #include "FXAS21000.h"
whatnick 15:b5a3e22d706b 4 #include "MBed_Adafruit_GPS.h"
whatnick 16:2e42284011d9 5 #include "Adafruit_SSD1351.h"
whatnick 16:2e42284011d9 6 //#include "NeatGUI.h"
whatnick 14:9885c8536437 7
whatnick 16:2e42284011d9 8 DigitalOut myled(LED1);
whatnick 16:2e42284011d9 9 DigitalOut BOOSTEN(PTC13); //oled power enable
whatnick 16:2e42284011d9 10
whatnick 14:9885c8536437 11 Serial gps(PTD3,PTD2);
whatnick 14:9885c8536437 12 Serial pc(USBTX, USBRX);
dan 0:7dec7e9ac085 13
whatnick 16:2e42284011d9 14 //SSD1351_SPI OLED96(PTB22,PTB23,PTB21,PTB20,PTD15); //Hexiwear pins for SPI bus OLED (MOSI, MISO, SCK, CS, DC)
whatnick 16:2e42284011d9 15 Adafruit_SSD1351 OLED96(PTB20,PTE6,PTD15,PTB21,PTB22); //Hexiwear pins for SPI bus OLED (CS, RS, DC, SCK, MOSI, MISO);
whatnick 16:2e42284011d9 16
whatnick 16:2e42284011d9 17 //#define BLACK 0x000000
whatnick 16:2e42284011d9 18 //#define WHITE 0xFFFFFF
whatnick 16:2e42284011d9 19 #define GREEN 0x00FF00
whatnick 16:2e42284011d9 20 #define RED 0xFF0000
whatnick 16:2e42284011d9 21 #define BLUE 0x0000FF
whatnick 15:b5a3e22d706b 22
whatnick 14:9885c8536437 23 FXOS8700Q_acc acc( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
whatnick 14:9885c8536437 24 FXOS8700Q_mag mag( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
whatnick 14:9885c8536437 25 MotionSensorDataUnits mag_data;
whatnick 14:9885c8536437 26 MotionSensorDataUnits acc_data;
whatnick 14:9885c8536437 27
whatnick 14:9885c8536437 28 MotionSensorDataCounts mag_raw;
whatnick 14:9885c8536437 29 MotionSensorDataCounts acc_raw;
dan 0:7dec7e9ac085 30
whatnick 15:b5a3e22d706b 31 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
whatnick 15:b5a3e22d706b 32
dan 0:7dec7e9ac085 33 int main() {
whatnick 14:9885c8536437 34 float faX, faY, faZ;
whatnick 14:9885c8536437 35 float fmX, fmY, fmZ;
whatnick 14:9885c8536437 36 int16_t raX, raY, raZ;
whatnick 14:9885c8536437 37 int16_t rmX, rmY, rmZ;
whatnick 15:b5a3e22d706b 38 float gyro_data[3];
whatnick 15:b5a3e22d706b 39
whatnick 15:b5a3e22d706b 40 Adafruit_GPS myGPS(&gps);
whatnick 15:b5a3e22d706b 41 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
whatnick 15:b5a3e22d706b 42 myGPS.begin(115200);
whatnick 15:b5a3e22d706b 43 //Turn off all sentences except GGA and RMC
whatnick 15:b5a3e22d706b 44 //For MTK GPS
whatnick 15:b5a3e22d706b 45 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
whatnick 15:b5a3e22d706b 46
whatnick 15:b5a3e22d706b 47 //FOR UBLOX GPS
whatnick 15:b5a3e22d706b 48 myGPS.sendCommand(UBX_DISABLE_ZDA);
whatnick 15:b5a3e22d706b 49 myGPS.sendCommand(UBX_DISABLE_GLL);
whatnick 15:b5a3e22d706b 50 myGPS.sendCommand(UBX_DISABLE_VTG);
whatnick 15:b5a3e22d706b 51 myGPS.sendCommand(UBX_DISABLE_GSV);
whatnick 15:b5a3e22d706b 52 myGPS.sendCommand(UBX_DISABLE_GSA);
whatnick 15:b5a3e22d706b 53
whatnick 15:b5a3e22d706b 54
whatnick 16:2e42284011d9 55 pc.baud(9600);
whatnick 15:b5a3e22d706b 56
whatnick 14:9885c8536437 57 acc.enable();
whatnick 14:9885c8536437 58 pc.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
whatnick 15:b5a3e22d706b 59 pc.printf("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI());
whatnick 15:b5a3e22d706b 60
whatnick 16:2e42284011d9 61 BOOSTEN = 1;
whatnick 16:2e42284011d9 62
whatnick 16:2e42284011d9 63 OLED96.begin();
whatnick 16:2e42284011d9 64 //OLED96.invert(true);
whatnick 16:2e42284011d9 65 OLED96.setRotation(0);
whatnick 16:2e42284011d9 66 OLED96.setTextColor(OLED96.Color565(0,255,0));
whatnick 18:76f4fc030036 67 OLED96.setTextWrap(false);
whatnick 16:2e42284011d9 68 OLED96.on();
whatnick 16:2e42284011d9 69 OLED96.fillScreen(BLACK);
whatnick 18:76f4fc030036 70 /*
whatnick 18:76f4fc030036 71 for(int i=0;i<128;i++)
whatnick 18:76f4fc030036 72 {
whatnick 18:76f4fc030036 73 for(int j=0;j<128;j++)
whatnick 18:76f4fc030036 74 {
whatnick 18:76f4fc030036 75 OLED96.drawPixel(i,j,OLED96.Color565(0,255,0));
whatnick 18:76f4fc030036 76 wait(0.1);
whatnick 18:76f4fc030036 77 }
whatnick 18:76f4fc030036 78 }
whatnick 18:76f4fc030036 79 */
whatnick 18:76f4fc030036 80 //OLED96.fillScreen(BLACK);
whatnick 18:76f4fc030036 81 OLED96.printf("Hello from Hexi");
whatnick 16:2e42284011d9 82
whatnick 16:2e42284011d9 83 /*
whatnick 16:2e42284011d9 84 OLED96.open();
whatnick 16:2e42284011d9 85 OLED96.state(Display::DISPLAY_ON);
whatnick 16:2e42284011d9 86 OLED96.drawCircle(10,10,5,0xFFFFFFFF);
whatnick 16:2e42284011d9 87 OLED96.flush();
whatnick 16:2e42284011d9 88 wait(2.0);
whatnick 16:2e42284011d9 89 */
whatnick 16:2e42284011d9 90 //OLED128.state(Display::DISPLAY_OFF);
whatnick 16:2e42284011d9 91
whatnick 16:2e42284011d9 92 //BOOSTEN = 0;
whatnick 15:b5a3e22d706b 93
whatnick 14:9885c8536437 94 while (true) {
whatnick 16:2e42284011d9 95
whatnick 14:9885c8536437 96 acc.getAxis(acc_data);
whatnick 14:9885c8536437 97 mag.getAxis(mag_data);
whatnick 15:b5a3e22d706b 98 gyro.ReadXYZ(gyro_data);
whatnick 14:9885c8536437 99 pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", acc_data.x, acc_data.y, acc_data.z);
whatnick 14:9885c8536437 100 pc.printf(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
whatnick 15:b5a3e22d706b 101 pc.printf("FXAS21000 X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
whatnick 14:9885c8536437 102 acc.getX(&faX);
whatnick 14:9885c8536437 103 acc.getY(&faY);
whatnick 14:9885c8536437 104 acc.getZ(&faZ);
whatnick 14:9885c8536437 105 mag.getX(&fmX);
whatnick 14:9885c8536437 106 mag.getY(&fmY);
whatnick 14:9885c8536437 107 mag.getZ(&fmZ);
whatnick 14:9885c8536437 108 pc.printf("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", faX, faY, faZ);
whatnick 14:9885c8536437 109 pc.printf(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
whatnick 14:9885c8536437 110 acc.getAxis(acc_raw);
whatnick 14:9885c8536437 111 mag.getAxis(mag_raw);
whatnick 14:9885c8536437 112 pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d ", acc_raw.x, acc_raw.y, acc_raw.z);
whatnick 14:9885c8536437 113 pc.printf(" MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
whatnick 14:9885c8536437 114 acc.getX(&raX);
whatnick 14:9885c8536437 115 acc.getY(&raY);
whatnick 14:9885c8536437 116 acc.getZ(&raZ);
whatnick 14:9885c8536437 117 mag.getX(&rmX);
whatnick 14:9885c8536437 118 mag.getY(&rmY);
whatnick 14:9885c8536437 119 mag.getZ(&rmZ);
whatnick 14:9885c8536437 120 pc.printf("FXOS8700Q ACC: X=%d Y=%d Z=%d ", raX, raY, raZ);
whatnick 15:b5a3e22d706b 121 pc.printf(" MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
whatnick 15:b5a3e22d706b 122
whatnick 16:2e42284011d9 123 /*
whatnick 15:b5a3e22d706b 124 while(gps.readable())
whatnick 15:b5a3e22d706b 125 {
whatnick 15:b5a3e22d706b 126 c = myGPS.read(); //queries the GPS
whatnick 15:b5a3e22d706b 127
whatnick 15:b5a3e22d706b 128 if (c) {
whatnick 15:b5a3e22d706b 129 pc.printf("%c", c); //this line will echo the GPS data if not paused
whatnick 15:b5a3e22d706b 130 }
whatnick 15:b5a3e22d706b 131 }
whatnick 15:b5a3e22d706b 132
whatnick 15:b5a3e22d706b 133 //check if we recieved a new message from GPS, if so, attempt to parse it,
whatnick 15:b5a3e22d706b 134 if ( myGPS.newNMEAreceived() ) {
whatnick 15:b5a3e22d706b 135 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
whatnick 15:b5a3e22d706b 136 continue;
whatnick 15:b5a3e22d706b 137 }
whatnick 15:b5a3e22d706b 138 }
whatnick 16:2e42284011d9 139 */
whatnick 15:b5a3e22d706b 140
whatnick 15:b5a3e22d706b 141 wait(0.1);
stevep 4:81cea7a352b0 142 }
dan 0:7dec7e9ac085 143 }