Tisham Dhar / Mbed OS Hexi_GPSIMU_Hotshoe

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

main.cpp@22:e69bc54ca4c0, 2016-09-19 (annotated)

Committer:
whatnick
Date:
Mon Sep 19 04:59:45 2016 +0000
Revision:
22:e69bc54ca4c0
Parent:
21:b165e847c5ba
Child:
23:f170a1d72a84
Reformat display to fit

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 21:b165e847c5ba 5 #include "Hexi_OLED_SSD1351.h"
whatnick 14:9885c8536437 6
whatnick 16:2e42284011d9 7 DigitalOut myled(LED1);
whatnick 16:2e42284011d9 8
whatnick 14:9885c8536437 9 Serial gps(PTD3,PTD2);
whatnick 22:e69bc54ca4c0 10
whatnick 22:e69bc54ca4c0 11 //#define DEBUG
whatnick 22:e69bc54ca4c0 12
whatnick 22:e69bc54ca4c0 13 #ifdef DEBUG
whatnick 14:9885c8536437 14 Serial pc(USBTX, USBRX);
whatnick 22:e69bc54ca4c0 15 #define LOG(args...) pc.printf(args)
whatnick 22:e69bc54ca4c0 16 #else
whatnick 22:e69bc54ca4c0 17 #define LOG(args...)
whatnick 22:e69bc54ca4c0 18 #endif
dan 0:7dec7e9ac085 19
whatnick 21:b165e847c5ba 20 /* Instantiate the SSD1351 OLED Driver */
whatnick 21:b165e847c5ba 21 SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */
whatnick 16:2e42284011d9 22
whatnick 16:2e42284011d9 23 //#define BLACK 0x000000
whatnick 16:2e42284011d9 24 //#define WHITE 0xFFFFFF
whatnick 16:2e42284011d9 25 #define GREEN 0x00FF00
whatnick 16:2e42284011d9 26 #define RED 0xFF0000
whatnick 16:2e42284011d9 27 #define BLUE 0x0000FF
whatnick 15:b5a3e22d706b 28
whatnick 14:9885c8536437 29 FXOS8700Q_acc acc( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
whatnick 14:9885c8536437 30 FXOS8700Q_mag mag( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
whatnick 14:9885c8536437 31 MotionSensorDataUnits mag_data;
whatnick 14:9885c8536437 32 MotionSensorDataUnits acc_data;
whatnick 14:9885c8536437 33
whatnick 14:9885c8536437 34 MotionSensorDataCounts mag_raw;
whatnick 14:9885c8536437 35 MotionSensorDataCounts acc_raw;
dan 0:7dec7e9ac085 36
whatnick 15:b5a3e22d706b 37 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
whatnick 15:b5a3e22d706b 38
whatnick 21:b165e847c5ba 39 Timer t;
whatnick 21:b165e847c5ba 40 int lastUpdate;
whatnick 15:b5a3e22d706b 41
whatnick 21:b165e847c5ba 42 int main()
whatnick 21:b165e847c5ba 43 {
whatnick 21:b165e847c5ba 44 float faX, faY, faZ;
whatnick 21:b165e847c5ba 45 float fmX, fmY, fmZ;
whatnick 21:b165e847c5ba 46 int16_t raX, raY, raZ;
whatnick 21:b165e847c5ba 47 int16_t rmX, rmY, rmZ;
whatnick 21:b165e847c5ba 48 float gyro_data[3];
whatnick 21:b165e847c5ba 49
whatnick 21:b165e847c5ba 50 Adafruit_GPS myGPS(&gps);
whatnick 21:b165e847c5ba 51 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
whatnick 22:e69bc54ca4c0 52 char text[30]; /* Text Buffer */
whatnick 21:b165e847c5ba 53
whatnick 21:b165e847c5ba 54 //Start timer
whatnick 21:b165e847c5ba 55 t.start();
whatnick 21:b165e847c5ba 56
whatnick 21:b165e847c5ba 57 myGPS.begin(9600);
whatnick 15:b5a3e22d706b 58 //Turn off all sentences except GGA and RMC
whatnick 15:b5a3e22d706b 59 //For MTK GPS
whatnick 21:b165e847c5ba 60 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
whatnick 15:b5a3e22d706b 61
whatnick 15:b5a3e22d706b 62 //FOR UBLOX GPS
whatnick 21:b165e847c5ba 63 myGPS.sendCommand(UBX_DISABLE_ZDA);
whatnick 21:b165e847c5ba 64 myGPS.sendCommand(UBX_DISABLE_GLL);
whatnick 21:b165e847c5ba 65 myGPS.sendCommand(UBX_DISABLE_VTG);
whatnick 21:b165e847c5ba 66 myGPS.sendCommand(UBX_DISABLE_GSV);
whatnick 21:b165e847c5ba 67 myGPS.sendCommand(UBX_DISABLE_GSA);
whatnick 15:b5a3e22d706b 68
whatnick 15:b5a3e22d706b 69
whatnick 22:e69bc54ca4c0 70 #ifdef DEBUG
whatnick 21:b165e847c5ba 71 pc.baud(115200);
whatnick 22:e69bc54ca4c0 72 #endif
whatnick 21:b165e847c5ba 73
whatnick 21:b165e847c5ba 74 acc.enable();
whatnick 22:e69bc54ca4c0 75 LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
whatnick 22:e69bc54ca4c0 76 LOG("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI());
whatnick 21:b165e847c5ba 77
whatnick 21:b165e847c5ba 78 /* Get OLED Class Default Text Properties */
whatnick 21:b165e847c5ba 79 oled_text_properties_t textProperties = {0};
whatnick 21:b165e847c5ba 80 oled.GetTextProperties(&textProperties);
whatnick 15:b5a3e22d706b 81
whatnick 21:b165e847c5ba 82 /* Turn on the backlight of the OLED Display */
whatnick 21:b165e847c5ba 83 oled.DimScreenON();
whatnick 15:b5a3e22d706b 84
whatnick 21:b165e847c5ba 85 /* Fills the screen with solid black */
whatnick 21:b165e847c5ba 86 oled.FillScreen(COLOR_BLACK);
whatnick 21:b165e847c5ba 87
whatnick 21:b165e847c5ba 88 /* Display Text at (x=7,y=0) */
whatnick 21:b165e847c5ba 89 strcpy((char *) text,"AERO GPS-AHRS");
whatnick 21:b165e847c5ba 90 oled.Label((uint8_t *)text,7,0);
whatnick 16:2e42284011d9 91
whatnick 21:b165e847c5ba 92 /* Change font color to blue */
whatnick 21:b165e847c5ba 93 textProperties.fontColor = COLOR_BLUE;
whatnick 21:b165e847c5ba 94 oled.SetTextProperties(&textProperties);
whatnick 22:e69bc54ca4c0 95
whatnick 22:e69bc54ca4c0 96
whatnick 22:e69bc54ca4c0 97 /* Display text at (x=1,y=15) MAG label*/
whatnick 21:b165e847c5ba 98 strcpy(text,"M:");
whatnick 22:e69bc54ca4c0 99 oled.Label((uint8_t *)text,1,15);
whatnick 22:e69bc54ca4c0 100
whatnick 22:e69bc54ca4c0 101 /* Display text at (x=1,y=30) ACC label*/
whatnick 21:b165e847c5ba 102 strcpy(text,"A:");
whatnick 22:e69bc54ca4c0 103 oled.Label((uint8_t *)text,1,30);
whatnick 22:e69bc54ca4c0 104
whatnick 22:e69bc54ca4c0 105 /* Display text at (x=1,y=45) GYRO label*/
whatnick 21:b165e847c5ba 106 strcpy(text,"G:");
whatnick 22:e69bc54ca4c0 107 oled.Label((uint8_t *)text,1,45);
whatnick 21:b165e847c5ba 108
whatnick 22:e69bc54ca4c0 109 /* Display text at (x=1,y=60) Time label*/
whatnick 21:b165e847c5ba 110 strcpy(text,"Timer(s):");
whatnick 22:e69bc54ca4c0 111 oled.Label((uint8_t *)text,1,60);
whatnick 22:e69bc54ca4c0 112
whatnick 22:e69bc54ca4c0 113 /* Display text at (x=1,y=75) Position label*/
whatnick 21:b165e847c5ba 114 strcpy(text,"P:");
whatnick 22:e69bc54ca4c0 115 oled.Label((uint8_t *)text,1,75);
whatnick 21:b165e847c5ba 116
whatnick 21:b165e847c5ba 117 /* Set text properties to white and right aligned for the dynamic text */
whatnick 21:b165e847c5ba 118 textProperties.fontColor = COLOR_WHITE;
whatnick 21:b165e847c5ba 119 textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
whatnick 21:b165e847c5ba 120 oled.SetTextProperties(&textProperties);
whatnick 20:5a4e47822d79 121
whatnick 16:2e42284011d9 122
whatnick 21:b165e847c5ba 123 while (true) {
whatnick 15:b5a3e22d706b 124
whatnick 20:5a4e47822d79 125 c = myGPS.read(); //queries the GPS
whatnick 20:5a4e47822d79 126
whatnick 15:b5a3e22d706b 127 //check if we recieved a new message from GPS, if so, attempt to parse it,
whatnick 15:b5a3e22d706b 128 if ( myGPS.newNMEAreceived() ) {
whatnick 15:b5a3e22d706b 129 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
whatnick 15:b5a3e22d706b 130 continue;
whatnick 15:b5a3e22d706b 131 }
whatnick 15:b5a3e22d706b 132 }
whatnick 21:b165e847c5ba 133
whatnick 21:b165e847c5ba 134 int Now = t.read_ms();
whatnick 21:b165e847c5ba 135
whatnick 21:b165e847c5ba 136 if(Now - lastUpdate > 500) {
whatnick 21:b165e847c5ba 137
whatnick 21:b165e847c5ba 138 acc.getAxis(acc_data);
whatnick 21:b165e847c5ba 139 mag.getAxis(mag_data);
whatnick 21:b165e847c5ba 140 gyro.ReadXYZ(gyro_data);
whatnick 22:e69bc54ca4c0 141 LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", acc_data.x, acc_data.y, acc_data.z);
whatnick 22:e69bc54ca4c0 142 LOG(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
whatnick 22:e69bc54ca4c0 143 LOG("FXAS21000 X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
whatnick 21:b165e847c5ba 144 acc.getX(&faX);
whatnick 21:b165e847c5ba 145 acc.getY(&faY);
whatnick 21:b165e847c5ba 146 acc.getZ(&faZ);
whatnick 21:b165e847c5ba 147 mag.getX(&fmX);
whatnick 21:b165e847c5ba 148 mag.getY(&fmY);
whatnick 21:b165e847c5ba 149 mag.getZ(&fmZ);
whatnick 22:e69bc54ca4c0 150 LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f ", faX, faY, faZ);
whatnick 22:e69bc54ca4c0 151 LOG(" MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
whatnick 21:b165e847c5ba 152 acc.getAxis(acc_raw);
whatnick 21:b165e847c5ba 153 mag.getAxis(mag_raw);
whatnick 22:e69bc54ca4c0 154 LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d ", acc_raw.x, acc_raw.y, acc_raw.z);
whatnick 22:e69bc54ca4c0 155 LOG(" MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
whatnick 21:b165e847c5ba 156 acc.getX(&raX);
whatnick 21:b165e847c5ba 157 acc.getY(&raY);
whatnick 21:b165e847c5ba 158 acc.getZ(&raZ);
whatnick 21:b165e847c5ba 159 mag.getX(&rmX);
whatnick 21:b165e847c5ba 160 mag.getY(&rmY);
whatnick 21:b165e847c5ba 161 mag.getZ(&rmZ);
whatnick 22:e69bc54ca4c0 162 LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d ", raX, raY, raZ);
whatnick 22:e69bc54ca4c0 163 LOG(" MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
whatnick 22:e69bc54ca4c0 164
whatnick 21:b165e847c5ba 165 /* Format the MAG reading */
whatnick 21:b165e847c5ba 166 sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
whatnick 21:b165e847c5ba 167 /* Display Mag readings to 1 decimal */
whatnick 22:e69bc54ca4c0 168 oled.TextBox((uint8_t *)text,15,15,80,15); /*Expand textbox for more digits*/
whatnick 22:e69bc54ca4c0 169
whatnick 21:b165e847c5ba 170 /* Format the ACC reading */
whatnick 21:b165e847c5ba 171 sprintf(text,"%.2f %.2f %.2f",faX,faY,faZ);
whatnick 21:b165e847c5ba 172 /* Display acceleration to 2 decimal */
whatnick 22:e69bc54ca4c0 173 oled.TextBox((uint8_t *)text,15,30,80,15); /*Expand textbox for more digits*/
whatnick 22:e69bc54ca4c0 174
whatnick 21:b165e847c5ba 175 /* Format the GYRO reading */
whatnick 21:b165e847c5ba 176 sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
whatnick 21:b165e847c5ba 177 /* Display gyro to 2 decimal */
whatnick 22:e69bc54ca4c0 178 oled.TextBox((uint8_t *)text,15,45,80,15); /*Expand textbox for more digits*/
whatnick 22:e69bc54ca4c0 179
whatnick 22:e69bc54ca4c0 180
whatnick 21:b165e847c5ba 181 /* Format the time reading */
whatnick 21:b165e847c5ba 182 sprintf(text,"%.2f",t.read());
whatnick 22:e69bc54ca4c0 183
whatnick 22:e69bc54ca4c0 184 /* Display time reading */
whatnick 22:e69bc54ca4c0 185 oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/
whatnick 22:e69bc54ca4c0 186
whatnick 21:b165e847c5ba 187 if(myGPS.fix) {
whatnick 22:e69bc54ca4c0 188 LOG(myGPS.lastNMEA());
whatnick 21:b165e847c5ba 189 sprintf(text,"%.2f %.2f %.2f",myGPS.longitude,myGPS.longitude,myGPS.altitude);
whatnick 22:e69bc54ca4c0 190 } else {
whatnick 21:b165e847c5ba 191 sprintf(text,"No GPS");
whatnick 21:b165e847c5ba 192 }
whatnick 21:b165e847c5ba 193
whatnick 22:e69bc54ca4c0 194 /* Display GPS location or lock status */
whatnick 22:e69bc54ca4c0 195 oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
whatnick 22:e69bc54ca4c0 196
whatnick 21:b165e847c5ba 197 lastUpdate = t.read_ms();
whatnick 21:b165e847c5ba 198 }
stevep 4:81cea7a352b0 199 }
dan 0:7dec7e9ac085 200 }