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@26:e35dd673fd13, 2017-02-02 (annotated)

Committer:
whatnick
Date:
Thu Feb 02 03:08:13 2017 +0000
Revision:
26:e35dd673fd13
Parent:
25:6e43bbe76aec 
Changed acc-mag library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan 0:7dec7e9ac085 1 #include "mbed.h"
whatnick 26:e35dd673fd13 2 #include "FXOS8700CQ.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 26:e35dd673fd13 6 #include "MadgwickAHRS.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 26:e35dd673fd13 38 FXOS8700CQ combo( PTC11, PTC10); // Proper Ports and I2C Address for Hexiwear
dan 0:7dec7e9ac085 39
whatnick 15:b5a3e22d706b 40 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
whatnick 15:b5a3e22d706b 41
whatnick 26:e35dd673fd13 42 Data combo_data;
whatnick 23:f170a1d72a84 43 float gyro_data[3];
whatnick 23:f170a1d72a84 44
whatnick 21:b165e847c5ba 45 Timer t;
whatnick 15:b5a3e22d706b 46
whatnick 25:6e43bbe76aec 47 //Worker threads
whatnick 25:6e43bbe76aec 48 Thread gps_t,imu_t;
whatnick 25:6e43bbe76aec 49
whatnick 25:6e43bbe76aec 50
whatnick 25:6e43bbe76aec 51 //GPS Reader worker task
whatnick 25:6e43bbe76aec 52 void gps_thread(void)
whatnick 23:f170a1d72a84 53 {
whatnick 23:f170a1d72a84 54 while (true) {
whatnick 23:f170a1d72a84 55 c = myGPS.read(); //queries the GPS
whatnick 24:cbdf0f7d33bd 56
whatnick 23:f170a1d72a84 57 //check if we recieved a new message from GPS, if so, attempt to parse it,
whatnick 23:f170a1d72a84 58 if ( myGPS.newNMEAreceived() ) {
whatnick 23:f170a1d72a84 59 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
whatnick 24:cbdf0f7d33bd 60 LOG(myGPS.lastNMEA());
whatnick 23:f170a1d72a84 61 continue;
whatnick 23:f170a1d72a84 62 }
whatnick 23:f170a1d72a84 63 }
whatnick 23:f170a1d72a84 64 Thread::wait(1);
whatnick 23:f170a1d72a84 65 }
whatnick 23:f170a1d72a84 66 }
whatnick 23:f170a1d72a84 67
whatnick 25:6e43bbe76aec 68 //IMU reader and sensor fusion worker task
whatnick 25:6e43bbe76aec 69 void imu_thread(void)
whatnick 23:f170a1d72a84 70 {
whatnick 23:f170a1d72a84 71 while(true) {
whatnick 23:f170a1d72a84 72 gyro.ReadXYZ(gyro_data);
whatnick 26:e35dd673fd13 73 combo_data = combo.get_values();
whatnick 25:6e43bbe76aec 74 Thread::wait(10);
whatnick 23:f170a1d72a84 75 }
whatnick 23:f170a1d72a84 76 }
whatnick 23:f170a1d72a84 77
whatnick 21:b165e847c5ba 78 int main()
whatnick 21:b165e847c5ba 79 {
whatnick 22:e69bc54ca4c0 80 char text[30]; /* Text Buffer */
whatnick 21:b165e847c5ba 81
whatnick 21:b165e847c5ba 82 //Start timer
whatnick 21:b165e847c5ba 83 t.start();
whatnick 21:b165e847c5ba 84
whatnick 21:b165e847c5ba 85 myGPS.begin(9600);
whatnick 15:b5a3e22d706b 86 //Turn off all sentences except GGA and RMC
whatnick 15:b5a3e22d706b 87 //For MTK GPS
whatnick 21:b165e847c5ba 88 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
whatnick 15:b5a3e22d706b 89
whatnick 15:b5a3e22d706b 90 //FOR UBLOX GPS
whatnick 21:b165e847c5ba 91 myGPS.sendCommand(UBX_DISABLE_ZDA);
whatnick 21:b165e847c5ba 92 myGPS.sendCommand(UBX_DISABLE_GLL);
whatnick 21:b165e847c5ba 93 myGPS.sendCommand(UBX_DISABLE_VTG);
whatnick 21:b165e847c5ba 94 myGPS.sendCommand(UBX_DISABLE_GSV);
whatnick 21:b165e847c5ba 95 myGPS.sendCommand(UBX_DISABLE_GSA);
whatnick 15:b5a3e22d706b 96
whatnick 15:b5a3e22d706b 97
whatnick 22:e69bc54ca4c0 98 #ifdef DEBUG
whatnick 21:b165e847c5ba 99 pc.baud(115200);
whatnick 22:e69bc54ca4c0 100 #endif
whatnick 21:b165e847c5ba 101
whatnick 25:6e43bbe76aec 102 //Connect to Nikon D800 UART at 4800baud
whatnick 25:6e43bbe76aec 103 camera.baud(4800);
whatnick 25:6e43bbe76aec 104
whatnick 26:e35dd673fd13 105 //Initialise gyro
whatnick 26:e35dd673fd13 106 char whoami = gyro.getWhoAmI();
whatnick 26:e35dd673fd13 107 //Initialise combo (acc&mag)
whatnick 26:e35dd673fd13 108 combo.init();
whatnick 21:b165e847c5ba 109
whatnick 21:b165e847c5ba 110 /* Get OLED Class Default Text Properties */
whatnick 21:b165e847c5ba 111 oled_text_properties_t textProperties = {0};
whatnick 21:b165e847c5ba 112 oled.GetTextProperties(&textProperties);
whatnick 15:b5a3e22d706b 113
whatnick 21:b165e847c5ba 114 /* Turn on the backlight of the OLED Display */
whatnick 21:b165e847c5ba 115 oled.DimScreenON();
whatnick 15:b5a3e22d706b 116
whatnick 21:b165e847c5ba 117 /* Fills the screen with solid black */
whatnick 21:b165e847c5ba 118 oled.FillScreen(COLOR_BLACK);
whatnick 21:b165e847c5ba 119
whatnick 23:f170a1d72a84 120 /* Adding full screen splash */
whatnick 23:f170a1d72a84 121 const uint8_t *image = NXP_whole_bmp;
whatnick 23:f170a1d72a84 122 /* Draws the image on the Screen starting at (x=0,y=0) */
whatnick 23:f170a1d72a84 123 oled.DrawImage(image,0,0);
whatnick 23:f170a1d72a84 124
whatnick 23:f170a1d72a84 125 Thread::wait(1000);
whatnick 23:f170a1d72a84 126
whatnick 23:f170a1d72a84 127 /* Fills the screen with solid black */
whatnick 23:f170a1d72a84 128 oled.FillScreen(COLOR_BLACK);
whatnick 23:f170a1d72a84 129
whatnick 21:b165e847c5ba 130 /* Display Text at (x=7,y=0) */
whatnick 21:b165e847c5ba 131 strcpy((char *) text,"AERO GPS-AHRS");
whatnick 21:b165e847c5ba 132 oled.Label((uint8_t *)text,7,0);
whatnick 16:2e42284011d9 133
whatnick 21:b165e847c5ba 134 /* Change font color to blue */
whatnick 21:b165e847c5ba 135 textProperties.fontColor = COLOR_BLUE;
whatnick 21:b165e847c5ba 136 oled.SetTextProperties(&textProperties);
whatnick 22:e69bc54ca4c0 137
whatnick 22:e69bc54ca4c0 138
whatnick 22:e69bc54ca4c0 139 /* Display text at (x=1,y=15) MAG label*/
whatnick 21:b165e847c5ba 140 strcpy(text,"M:");
whatnick 22:e69bc54ca4c0 141 oled.Label((uint8_t *)text,1,15);
whatnick 22:e69bc54ca4c0 142
whatnick 22:e69bc54ca4c0 143 /* Display text at (x=1,y=30) ACC label*/
whatnick 21:b165e847c5ba 144 strcpy(text,"A:");
whatnick 22:e69bc54ca4c0 145 oled.Label((uint8_t *)text,1,30);
whatnick 22:e69bc54ca4c0 146
whatnick 22:e69bc54ca4c0 147 /* Display text at (x=1,y=45) GYRO label*/
whatnick 21:b165e847c5ba 148 strcpy(text,"G:");
whatnick 22:e69bc54ca4c0 149 oled.Label((uint8_t *)text,1,45);
whatnick 21:b165e847c5ba 150
whatnick 22:e69bc54ca4c0 151 /* Display text at (x=1,y=60) Time label*/
whatnick 21:b165e847c5ba 152 strcpy(text,"Timer(s):");
whatnick 22:e69bc54ca4c0 153 oled.Label((uint8_t *)text,1,60);
whatnick 22:e69bc54ca4c0 154
whatnick 22:e69bc54ca4c0 155 /* Display text at (x=1,y=75) Position label*/
whatnick 21:b165e847c5ba 156 strcpy(text,"P:");
whatnick 22:e69bc54ca4c0 157 oled.Label((uint8_t *)text,1,75);
whatnick 21:b165e847c5ba 158
whatnick 21:b165e847c5ba 159 /* Set text properties to white and right aligned for the dynamic text */
whatnick 21:b165e847c5ba 160 textProperties.fontColor = COLOR_WHITE;
whatnick 21:b165e847c5ba 161 textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
whatnick 21:b165e847c5ba 162 oled.SetTextProperties(&textProperties);
whatnick 20:5a4e47822d79 163
whatnick 24:cbdf0f7d33bd 164 //Start read and update threads
whatnick 25:6e43bbe76aec 165 gps_t.start(gps_thread);
whatnick 25:6e43bbe76aec 166 imu_t.start(imu_thread);
whatnick 16:2e42284011d9 167
whatnick 23:f170a1d72a84 168 while (true) {
whatnick 23:f170a1d72a84 169 /* Format the GYRO reading */
whatnick 23:f170a1d72a84 170 sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
whatnick 23:f170a1d72a84 171 /* Display gyro to 2 decimal */
whatnick 23:f170a1d72a84 172 oled.TextBox((uint8_t *)text,15,45,80,15); /*Expand textbox for more digits*/
whatnick 23:f170a1d72a84 173
whatnick 23:f170a1d72a84 174
whatnick 23:f170a1d72a84 175 /* Format the time reading */
whatnick 23:f170a1d72a84 176 sprintf(text,"%.2f",t.read());
whatnick 23:f170a1d72a84 177
whatnick 23:f170a1d72a84 178 /* Display time reading */
whatnick 23:f170a1d72a84 179 oled.TextBox((uint8_t *)text,55,60,35,15); /*Expand textbox for more digits*/
whatnick 23:f170a1d72a84 180
whatnick 23:f170a1d72a84 181 if(myGPS.fix) {
whatnick 23:f170a1d72a84 182 LOG(myGPS.lastNMEA());
whatnick 23:f170a1d72a84 183 sprintf(text,"%.2f%c %.2f%c",myGPS.longitude/100.0f,myGPS.lat,myGPS.latitude/100.0f,myGPS.lon);
whatnick 23:f170a1d72a84 184 } else {
whatnick 23:f170a1d72a84 185 sprintf(text,"No GPS");
whatnick 15:b5a3e22d706b 186 }
whatnick 21:b165e847c5ba 187
whatnick 23:f170a1d72a84 188 /* Display GPS location or lock status */
whatnick 23:f170a1d72a84 189 oled.TextBox((uint8_t *)text,15,75,80,15); /*Expand textbox for more digits*/
whatnick 23:f170a1d72a84 190 Thread::wait(1000);
stevep 4:81cea7a352b0 191 }
dan 0:7dec7e9ac085 192 }