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@25:6e43bbe76aec, 2016-09-24 (annotated)

Committer:
whatnick
Date:
Sat Sep 24 06:10:54 2016 +0000
Revision:
25:6e43bbe76aec
Parent:
24:cbdf0f7d33bd
Child:
26:e35dd673fd13
Add BLE/haptic module as dependency and start threads properly.

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