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
Diff: main.cpp
- Revision:
- 25:6e43bbe76aec
- Parent:
- 24:cbdf0f7d33bd
- Child:
- 26:e35dd673fd13
diff -r cbdf0f7d33bd -r 6e43bbe76aec main.cpp --- a/main.cpp Tue Sep 20 04:18:34 2016 +0000 +++ b/main.cpp Sat Sep 24 06:10:54 2016 +0000 @@ -12,6 +12,8 @@ DigitalOut myled(LED1); Serial gps(PTD3,PTD2); +Serial camera(PTC17,PTC16); + Adafruit_GPS myGPS(&gps); char c; //when read via Adafruit_GPS::read(), the class returns single character stored here @@ -51,7 +53,12 @@ Timer t; -void gps_thread(void const *args) +//Worker threads +Thread gps_t,imu_t; + + +//GPS Reader worker task +void gps_thread(void) { while (true) { c = myGPS.read(); //queries the GPS @@ -67,7 +74,8 @@ } } -void imu_thread(void const *args) +//IMU reader and sensor fusion worker task +void imu_thread(void) { while(true) { acc.getAxis(acc_data); @@ -104,7 +112,12 @@ LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d ", raX, raY, raZ); LOG(" MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ); - Thread::wait(100); + MadgwickQuaternionUpdate(faX,faY,faZ,gyro_data[0],gyro_data[1],gyro_data[2],fmX,fmY,fmZ); + calcEuler(); + + LOG("EULER: Y=%f R=%f P=%f\n", yaw,roll,pitch); + + Thread::wait(10); } } @@ -132,6 +145,9 @@ pc.baud(115200); #endif +//Connect to Nikon D800 UART at 4800baud + camera.baud(4800); + acc.enable(); LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI()); LOG("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI()); @@ -191,8 +207,8 @@ oled.SetTextProperties(&textProperties); //Start read and update threads - Thread gps_t(gps_thread); - Thread imu_t(imu_thread); + gps_t.start(gps_thread); + imu_t.start(imu_thread); while (true) { /* Format the MAG reading */