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@21:b165e847c5ba, 2016-09-19 (annotated)

Committer:
whatnick
Date:
Mon Sep 19 04:34:11 2016 +0000
Revision:
21:b165e847c5ba
Parent:
20:5a4e47822d79
Child:
22:e69bc54ca4c0
Read all sensors and display on OLED

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