Tisham Dhar / Hexi_GPSIMU_Hotshoe

Use hexiwear as a GPSIMU-AHRS for Nikon DSLR cameras

Dependencies:   FXOS8700Q FXAS21000 MBed_Adafruit-GPS-Library Hexi_OLED_SSD1351

Fork of Hexi_Blinky_Example by Hexiwear

/media/uploads/whatnick/hexiwear_docking_station_numbers.jpg

Committer:
whatnick
Date:
Tue Sep 20 04:18:34 2016 +0000
Revision:
24:cbdf0f7d33bd
Parent:
23:f170a1d72a84
Added sensor fusion code

Who changed what in which revision?

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