Minor fixes

Dependencies:   LSM9DS1_Library SDFileSystem mbed nrf51_rtc

Fork of LSM303DLHTest by Toshihisa T

Files at this revision

API Documentation at this revision

Comitter:
afmiee
Date:
Thu Sep 29 22:03:48 2016 +0000
Parent:
11:a246c67d44b0
Commit message:
Minor fixes

Changed in this revision

LSM9DS1_Library.lib Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
temp.txt Show annotated file Show diff for this revision Revisions of this file
diff -r a246c67d44b0 -r f361ccfc191e LSM9DS1_Library.lib
--- a/LSM9DS1_Library.lib	Wed Jul 27 21:55:46 2016 +0000
+++ b/LSM9DS1_Library.lib	Thu Sep 29 22:03:48 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/jmar7/code/LSM9DS1_Library/#58710c38076c
+https://developer.mbed.org/users/afmiee/code/LSM9DS1_Library/#58710c38076c
diff -r a246c67d44b0 -r f361ccfc191e SDFileSystem.lib
--- a/SDFileSystem.lib	Wed Jul 27 21:55:46 2016 +0000
+++ b/SDFileSystem.lib	Thu Sep 29 22:03:48 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/mbed_official/code/SDFileSystem/#c0b11a180d6f
+https://developer.mbed.org/users/afmiee/code/SDFileSystem/#c0b11a180d6f
diff -r a246c67d44b0 -r f361ccfc191e main.cpp
--- a/main.cpp	Wed Jul 27 21:55:46 2016 +0000
+++ b/main.cpp	Thu Sep 29 22:03:48 2016 +0000
@@ -10,7 +10,7 @@
 
 #define M_PI 3.14158
 //#define DEBUG 1     // Print Debug information to serial terminal
-#define MOTION 1    // Define if operated by motion or by buttons
+//#define MOTION 1    // Define if operated by motion or by buttons
 
 // define threshold and Duration registers for interrupts
 #define ACT_THS_REG 0x10                // 0x04
diff -r a246c67d44b0 -r f361ccfc191e temp.txt
--- a/temp.txt	Wed Jul 27 21:55:46 2016 +0000
+++ b/temp.txt	Thu Sep 29 22:03:48 2016 +0000
@@ -1,3 +1,80 @@
+// Latch Inc.
+// Antonio F Mondragon
+// 20160714
+// for the Adafruit 9DOF Modulke and the Sparkfun microSD card shield
+
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "SDFileSystem.h"
+#include "nrf51_rtc.h"
+
+#define M_PI 3.14158
+//#define DEBUG 1     // Print Debug information to serial terminal
+//#define MOTION 1    // Define if operated by motion or by buttons
+
+// define threshold and Duration registers for interrupts
+#define ACT_THS_REG 0x10                // 0x04
+#define ACT_DUR_REG 0x02                // 0x05
+
+#define INT_GEN_THS_X_XL_REG 0x0C       // 0x07
+#define INT_GEN_THS_Y_YL_REG 0xFF       // 0x08
+#define INT_GEN_THS_Z_XL_REG 0x0C       // 0x09
+#define INT_GEN_DUR_XL_REG 0x00         // 0x0A
+
+#define INT_GEN_THS_XHL_G_REG 0x0300    // 0x31-32
+#define INT_GEN_THS_YHL_G_REG 0x0300    // 0x33-34
+#define INT_GEN_THS_ZHL_G_REG 0x0300    // 0x35-36
+#define INT_GEN_DUR_G_REG 0x02          // 0x37
+
+#define LED_ON 0
+#define LED_OFF 01
+#define DBG_ACTIVE 0
+#define DBG_INACTIVE 1
+
+typedef unsigned long int ulint;
+
+// Create objects
+Serial debug(USBTX,USBRX);
+// For Nordic
+LSM9DS1 lol(p30, p7, 0xD6, 0x3C);
+I2C i2c(p30, p7);
+
+// Create the SD filesystem
+SDFileSystem sd(p25, p28, p29, p20, "sd"); // MOSI, MISO, SCLK, SSEL
+
+// Create a ticker to use the nRF51 RTC
+Ticker flipper;
+
+#ifdef MOTION
+// Assign interrupts to Interrupts from LSM9DS1
+InterruptIn int1(p12); // Start sampling
+InterruptIn int2(p13); // Stop sampling
+#else
+// Assign interrupts to Buttons 1 and 2 on nrf51-DK
+InterruptIn but1(p17); // Start sampling
+InterruptIn but2(p18); // Stop sampling
+#endif
+
+// LED definitions
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+DigitalOut dbg1(p24);
+DigitalOut dbg2(p0);
+
+// Global variables
+volatile int start = 0;
+volatile int stop = 0;
+time_t seconds;
+
+FILE *fpA;
+FILE *fpG;
+FILE *fpM;
+
+char filename[80];
+char secs_str[80];
+
 uint8_t I2CreadByte(uint8_t address, uint8_t subAddress);
 uint8_t I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count);
 int file_rename(const char *oldfname, const char *newfname);
@@ -12,5 +89,685 @@
 void DumpAccelGyroRegs( void );
 
 
+int main()
+{
+    led1 = 1;
+    led2 = 1;
+
+    // debug pins
+    dbg1 = DBG_INACTIVE;
+    dbg2 = DBG_INACTIVE;
+
+    struct tm t;
+    start = 0;
+    stop = 0;
+
+#ifdef MOTION
+    int1.fall(&start_smpl);
+    int2.fall(&stop_smpl);
+#else
+    but1.rise(&start_smpl);
+    but2.rise(&stop_smpl);
+#endif
+
+    // Attach functions to interrupts
+    flipper.attach(&flip, 1.0); // the address of the function to be attached (flip) and the interval (2 seconds)
+
+    // Enable serial port
+    debug.format(8,Serial::None,1);
+    debug.baud(115200);
+
+#ifdef DEBUG
+    debug.printf("LSM9DS1 Test\x0d\x0a");
+#endif
+
+    // Initialize 9DOF
+    if (!lol.begin()) {
+        debug.printf("Failed to communicate with LSM9DS1.\n");
+        wait(0.5);
+        while(1) {
+            led1 = !led1;
+            wait(0.5);
+        }
+    } else {
+        debug.printf("Communication with the LSM9DS1 successfully\n\r");
+    }
+
+    lol.calibrate(true);
+    lol.setFIFO(FIFO_CONT, 0x1F);
+ 
+    lol.getStatus();
+    lol.getAccelIntSrc();
+    lol.readAccel();
+    lol.readGyro();
+
+#ifdef MOTION
+    // Configure
+    lol.configAccelThs((uint8_t)INT_GEN_THS_X_XL_REG, X_AXIS,  (uint8_t)INT_GEN_DUR_XL_REG, false);                   // INT_GEN_THS_X_XL (07h)
+    lol.configAccelThs((uint8_t)INT_GEN_THS_Y_YL_REG, Y_AXIS,  (uint8_t)INT_GEN_DUR_XL_REG, false);                   // INT_GEN_THS_Y_XL (08h)
+    lol.configAccelThs((uint8_t)INT_GEN_THS_Z_XL_REG, Z_AXIS,  (uint8_t)INT_GEN_DUR_XL_REG, false);                   // INT_GEN_THS_Z_XL (09h)
+
+    lol.configGyroThs((int16_t )INT_GEN_THS_XHL_G_REG, X_AXIS, (uint8_t) INT_GEN_DUR_G_REG, false);                 // INT_GEN_THS_X_G (31h - 32h)
+    lol.configGyroThs((int16_t )INT_GEN_THS_YHL_G_REG, Y_AXIS, (uint8_t) INT_GEN_DUR_G_REG, false);                 // INT_GEN_THS_Y_G (33h - 34h)
+    lol.configGyroThs((int16_t )INT_GEN_THS_ZHL_G_REG, Z_AXIS, (uint8_t) INT_GEN_DUR_G_REG, false);                 // INT_GEN_THS_Z_G (35h - 36h)
+    //lol.configGyroInt(ZHIE_G|YHIE_G|XHIE_G, false, false);                              // INT_GEN_CFG_G (30h)
+
+    lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+    lol.configInt(XG_INT2, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+#endif
+
+#ifdef DEBUG
+    print_config_int_registers();
+#endif
+// Dump all registers
+    //DumpAccelGyroRegs();
+//    // Initialize current time if needed
+//    printf("Enter current date and time:\n");
+//    printf("YYYY MM DD HH MM SS[enter]\n");
+//    scanf("%d %d %d %d %d %d", &t.tm_year, &t.tm_mon, &t.tm_mday
+//          , &t.tm_hour, &t.tm_min, &t.tm_sec);
+
+    // adjust for tm structure required values
+    t.tm_year = t.tm_year - 1900;
+    t.tm_mon = t.tm_mon - 1;
+
+    // set the time
+    rtc.set_time(mktime(&t));
+    // Set the interrupt service routines
+
+    // Wait for the start Signal generated by the accelerometer interrupt
+#ifdef MOTION
+    // Read the interrupt to clear it
+    lol.getStatus();
+    lol.getAccelIntSrc();
+    // Disable the interrupt
+    lol.configAccelInt(0, false);                                        // INT_GEN_CFG_XL (06h)
+    lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+#endif
 
 
+    while(1) {
+#ifdef MOTION
+        lol.getStatus();
+        lol.getAccelIntSrc();
+        lol.readAccel();
+        lol.getGyroIntSrc();
+        lol.readGyro();
+        // Disable the interrupt
+        lol.configAccelInt(0, false);                                        // INT_GEN_CFG_XL (06h)
+        lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+        // Read the interrupt to clear it
+#endif
+        // Create temporary files
+        led3 = LED_ON;
+        open_temp_files();
+        led3 = LED_OFF;
+#ifdef DEBUG
+        debug.printf( "\n\r");
+        debug.printf( "Status     (27h) %02x\n\r", lol.getStatus());      // STATUS_REG     (27h)
+        debug.printf( "GyroIntSrc (14h) %02x\n\r", lol.getGyroIntSrc());  // INT_GEN_SRC_G  (14h)
+        debug.printf( "AccelIntSrc(26h) %02x\n\r", lol.getAccelIntSrc()); // INT_GEN_SRC_XL (26h)
+        debug.printf( "MagIntSrc  (31h) %02x\n\r", lol.getMagIntSrc());   // INT_SRC_M      (31h)
+        debug.printf( "Inactivity (17h) %02x\n\r", lol.getInactivity());  // STATUS_REG     (17h)
+        debug.printf( "\n\r");
+#endif
+#ifdef DEBUG
+        print_config_int_registers();
+#endif
+#ifdef MOTION
+        // Program the motion interrupt on accelerometer
+        lol.configAccelInt(ZHIE_XL|YHIE_XL|XHIE_XL, false);                  // INT_GEN_CFG_XL (06h)
+        lol.configInt(XG_INT1, INT1_IG_XL, INT_ACTIVE_LOW, INT_PUSH_PULL);   // INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+#endif
+        // Check for button 1 pressed or
+        // Wait for the start Signal generated by the accelerometer interrupt
+        // Depends if MOTION is defined
+#ifdef DEBUG
+#ifdef MOTION
+        debug.printf("Waiting for motion to start sampling\n\r");
+#else
+        debug.printf("Waiting for Button 1 to be pressed to start sampling\n\r");
+#endif
+#endif
+        while(start==0);
+        dbg1 = DBG_ACTIVE;
+        dbg2 = DBG_INACTIVE;
+#ifdef DEBUG
+#ifdef MOTION
+        debug.printf("Motion Detected\n\r");
+#else
+        debug.printf("Button 1 pressed\n\r");
+#endif
+#endif
+#ifdef MOTION
+        // Reset the Interrupt
+        lol.getStatus();
+        lol.getAccelIntSrc();
+
+        // Disable the interrupt
+        lol.configAccelInt(0, false);                                        // INT_GEN_CFG_XL (06h)
+        lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
+        // Read the interrupt to clear it
+#endif
+#ifdef DEBUG
+        debug.printf("Started sampling\n\r");
+        // Get the time and create a file with the number of seconds in hex appended
+#endif
+        // Caoture the seconds since Turn-on to name the files
+        seconds = rtc.time();
+        sprintf(secs_str, "%s", ctime(&seconds));
+#ifdef DEBUG
+        debug.printf("\n\rStarted at: %s\n\r\n\r", secs_str );
+#endif
+#ifdef MOTION
+        // program inactivity timer
+        lol.configInactivity(ACT_THS_REG, ACT_DUR_REG, true);
+        lol.configInt(XG_INT2, INT2_INACT, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT2_CTRL (0x0D) + CTRL_REG8 (0x22)
+#endif
+        //Execute until inactivity timer is triggered
+#ifdef DEBUG
+        print_config_int_registers();
+#endif
+
+        while(!stop) {
+            dbg1 = DBG_INACTIVE;
+            dbg2 = DBG_ACTIVE;
+            if (lol.accelAvailable()) {
+                lol.readAccel();
+#ifdef DEBUG
+                debug.printf("ACC %d, %d, %d\n\r", lol.ax, lol.ay, lol.az);
+#endif
+                fprintf(fpA, "%d, %d, %d\n\r", lol.ax, lol.ay, lol.az);
+            }
+            if ( lol.magAvailable(X_AXIS) && lol.magAvailable(Y_AXIS) && lol.magAvailable(Z_AXIS)) {
+                lol.readMag();
+#ifdef DEBUG
+                debug.printf("MAG %d, %d, %d\n\r", lol.mx, lol.my, lol.mz);
+#endif
+                fprintf(fpM, "%d, %d, %d\n\r", lol.mx, lol.my, lol.mz);
+            }
+            if ( lol.gyroAvailable()) {
+                lol.readGyro();
+#ifdef DEBUG
+                debug.printf("GYR %d, %d, %d\n\r", lol.gx, lol.gy, lol.gz);
+#endif
+                fprintf(fpG, "%d, %d, %d\n\r", lol.gx, lol.gy, lol.gz);
+            }
+        }
+#ifdef MOTION
+        //Disable inactivity interrupt 
+        lol.configInt(XG_INT2, 0, INT_ACTIVE_LOW, INT_PUSH_PULL);   //INT2_CTRL (0x0D) + CTRL_REG8 (0x22)
+#endif 
+        // Stop Sampling and close file
+        //parp(10);
+#ifdef DEBUG
+        debug.printf("Stopped sampling\n\r");
+#endif
+        led3 = LED_ON;
+        fclose(fpA);
+        fclose(fpM);
+        fclose(fpG);
+        rename_files();
+        led3 = LED_OFF;
+    }
+}
+
+uint8_t I2CreadByte(uint8_t address, uint8_t subAddress)
+{
+    char data;
+    char temp= subAddress;
+
+    i2c.write(address, &temp, 1);
+    int a = i2c.read(address, &data, 1);
+    return data;
+}
+
+uint8_t I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
+{
+    int i;
+    char temp_dest[count];
+    char temp = subAddress;
+    i2c.write(address, &temp, 1);
+    i2c.read(address, temp_dest, count);
+
+    //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion
+    for (i=0; i < count; i++) {
+        dest[i] = temp_dest[i];
+    }
+    return count;
+}
+//***********************************************************
+// file_rename: renames a file (via copy & delete).
+//    Moves data instead of adjusting the file name in the
+//    file directory. Checks to insure the file was renamed.
+//    Returns -1 = error; 0 = success
+//***********************************************************
+int file_rename(const char *oldfname, const char *newfname)
+{
+    int retval = 0;
+    int ch;
+
+    FILE *fpold = fopen(oldfname, "r");   // src file
+    FILE *fpnew = fopen(newfname, "w");   // dest file
+
+    while (1) {                   // Copy src to dest
+        ch = fgetc(fpold);        // until src EOF read.
+        if (ch == EOF) break;
+        fputc(ch, fpnew);
+    }
+
+    fclose(fpnew);
+    fclose(fpold);
+
+    fpnew = fopen(newfname, "r"); // Reopen dest to insure
+    if(fpnew == NULL) {           // that it was created.
+        retval = (-1);            // Return Error.
+    } else {
+        fclose(fpnew);
+        remove(oldfname);         // Remove original file.
+        retval = (0);             // Return Success.
+    }
+    return (retval);
+}
+
+//***********************************************************
+// file_copy: Copies a file
+//            Checks to insure destination file was created.
+//            Returns -1 = error; 0 = success
+//***********************************************************
+int file_copy (const char *src, const char *dst)
+{
+    int retval = 0;
+    int ch;
+
+    FILE *fpsrc = fopen(src, "r");   // src file
+    FILE *fpdst = fopen(dst, "w");   // dest file
+
+    while (1) {                  // Copy src to dest
+        ch = fgetc(fpsrc);       // until src EOF read.
+        if (ch == EOF) break;
+        fputc(ch, fpdst);
+    }
+    fclose(fpsrc);
+    fclose(fpdst);
+
+    fpdst = fopen(dst, "r");     // Reopen dest to insure
+    if(fpdst == NULL) {          // that it was created.
+        retval = (-1);           // Return error.
+    } else {
+        fclose(fpdst);
+        retval = (0);            // Return success.
+    }
+    return (retval);
+}
+// Generated when button 1 is pressed on rising edge START
+// Modified to be generated by Interrupt 1
+void start_smpl()
+{
+    start = 1;
+    stop = 0;
+    //dbg1 = 1;
+    //dbg2 = 0;
+}
+
+// Generated when button 1 is pressed on rising edge STOP
+// Modified to be generated by Interrupt 2
+void stop_smpl()
+{
+    stop = 1;
+    start = 0;
+    //dbg1 = 0;
+    //dbg2 = 1;
+}
+
+void parp( int times )
+{
+    int i;
+    for( i = 0; i < times; i++) {
+        led1 = LED_ON;
+        wait( 0.05);
+        led1 = LED_OFF;
+        wait( 0.05);
+    }
+    led2 = 1;
+}
+
+// Flipped every second
+void flip()
+{
+    led2 = LED_ON;
+    wait(0.01);
+    led2 = LED_OFF;
+    if ( start && !stop ) {
+        led1 = LED_ON;
+        wait(0.01);
+        led1 = LED_OFF;
+    }
+}
+
+void print_config_int_registers( void )
+{
+#ifdef DEBUG
+    debug.printf( "\n\r");
+    debug.printf( "INT1_CTRL        (0Ch) %02x\n\r", I2CreadByte(0xD6, 0x0C));
+    debug.printf( "INT2_CTRL        (0Dh) %02x\n\r", I2CreadByte(0xD6, 0x0D));
+    debug.printf( "CTRL_REG8        (22h) %02x\n\r", I2CreadByte(0xD6, 0x22));
+    debug.printf( "STATUS_REG       (27h) %02x\n\r", I2CreadByte(0xD6, 0x27));
+    debug.printf( "\n\r");
+    debug.printf( "INT_GEN_CFG_XL   (06h) %02x\n\r", I2CreadByte(0xD6, 0x06));
+    debug.printf( "INT_GEN_SRC_XL   (26h) %02x\n\r", I2CreadByte(0xD6, 0x26));
+    debug.printf( "INT_GEN_THS_X_XL (07h) %02x\n\r", I2CreadByte(0xD6, 0x07));
+    debug.printf( "INT_GEN_THS_Y_XL (08h) %02x\n\r", I2CreadByte(0xD6, 0x08));
+    debug.printf( "INT_GEN_THS_Z_XL (09h) %02x\n\r", I2CreadByte(0xD6, 0x09));
+    debug.printf( "INT_GEN_DUR_XL   (0ah) %02x\n\r", I2CreadByte(0xD6, 0x0a));
+    debug.printf( "\n\r");
+    debug.printf( "INT_GEN_CFG_G    (30h) %02x\n\r", I2CreadByte(0xD6, 0x30));
+    debug.printf( "INT_GEN_SRC_G    (14h) %02x\n\r", I2CreadByte(0xD6, 0x14));
+    debug.printf( "INT_GEN_THS_XH_G (31h) %02x\n\r", I2CreadByte(0xD6, 0x31));
+    debug.printf( "INT_GEN_THS_XL_G (32h) %02x\n\r", I2CreadByte(0xD6, 0x32));
+    debug.printf( "INT_GEN_THS_YH_G (33h) %02x\n\r", I2CreadByte(0xD6, 0x33));
+    debug.printf( "INT_GEN_THS_YL_G (34h) %02x\n\r", I2CreadByte(0xD6, 0x34));
+    debug.printf( "INT_GEN_THS_ZH_G (35h) %02x\n\r", I2CreadByte(0xD6, 0x35));
+    debug.printf( "INT_GEN_THS_ZL_G (36h) %02x\n\r", I2CreadByte(0xD6, 0x36));
+    debug.printf( "INT_GEN_DUR_G    (37h) %02x\n\r", I2CreadByte(0xD6, 0x37));
+#endif
+}
+
+void open_temp_files( void )
+{
+    debug.printf("\n\r");
+    fpA = fopen("/sd/ACC.csv", "w");
+    // Verify that file can be created
+    if ( fpA == NULL ) {
+        debug.printf("Cannot create file ACC.csv\n\r");
+        wait(0.5);
+        while(1) {
+            led1 = !led1;
+            wait(0.5);
+        }
+    } else
+        debug.printf("File ACC.csv created successfully\n\r");
+
+    fpG = fopen("/sd/GYR.csv", "w");
+    // Verify that file can be created
+    if ( fpG == NULL ) {
+        debug.printf("Cannot create file GYR.csv\n\r");
+        wait(0.5);
+        while(1) {
+            led1 = !led1;
+            wait(0.5);
+        }
+    } else
+        debug.printf("File GYR.csv created successfully\n\r");
+
+    fpM = fopen("/sd/MAG.csv", "w");
+    // Verify that file can be created
+    if ( fpM == NULL ) {
+        debug.printf("Cannot create file MAG.csv\n\r");
+        wait(0.5);
+        while(1) {
+            led1 = !led1;
+            wait(0.5);
+        }
+    } else
+        debug.printf("File MAG.csv created successfully\n\r");
+}
+
+void rename_files( void )
+{
+    sprintf(filename, "/sd/latch9DOFA_%08x.csv",seconds);
+    debug.printf("\n\r");
+    if((file_copy("/sd/ACC.csv",filename )) == 0) {
+        debug.printf("File ACC.csv copied successfully to %s\n\r", filename);
+    } else {
+        debug.printf("Error: unable to copy the file ACC.csv");
+    }
+    sprintf(filename, "/sd/latch9DOFM_%08x.csv",seconds);
+    if((file_copy("/sd/MAG.csv",filename )) == 0) {
+        debug.printf("File MAG.csv copied successfully to %s\n\r", filename);
+    } else {
+        debug.printf("Error: unable to copy the file MAG.csv");
+    }
+    sprintf(filename, "/sd/latch9DOFG_%08x.csv",seconds);
+    if((file_copy("/sd/GYR.csv",filename )) == 0) {
+        debug.printf("File GYR.csv copied successfully to %s\n\r", filename);
+    } else {
+        debug.printf("Error: unable to copy the file GYR.csv");
+    }
+}
+
+void DumpAccelGyroRegs( void )
+{
+    char dest[ 0x34 ];
+    int i;
+
+    debug.printf("\n\r");
+    I2CreadBytes( 0xD6, 0x04, (uint8_t *)dest, 0x34 );
+
+    for( i = 0; i < 0x34; i++ ) {
+        //I2CreadByte(0xD6, i + 0x04);
+        switch( i + 0x04 ) {
+            case 0x04:
+                debug.printf("ACT_THS             0x04 %02x\n\r", dest[i]);
+                break;
+            case 0x05:
+                debug.printf("ACT_DUR             0x05 %02x\n\r", dest[i]);
+                break;
+            case 0x06:
+                debug.printf("INT_GEN_CFG_XL      0x06 %02x\n\r", dest[i]);
+                break;
+            case 0x07:
+                debug.printf("INT_GEN_THS_X_XL    0x07 %02x\n\r", dest[i]);
+                break;
+            case 0x08:
+                debug.printf("INT_GEN_THS_Y_XL    0x08 %02x\n\r", dest[i]);
+                break;
+            case 0x09:
+                debug.printf("INT_GEN_THS_Z_XL    0x09 %02x\n\r", dest[i]);
+                break;
+            case 0x0A:
+                debug.printf("INT_GEN_DUR_XL      0x0A %02x\n\r", dest[i]);
+                break;
+            case 0x0B:
+                debug.printf("REFERENCE_G         0x0B %02x\n\r", dest[i]);
+                break;
+            case 0x0C:
+                debug.printf("INT1_CTRL           0x0C %02x\n\r", dest[i]);
+                break;
+            case 0x0D:
+                debug.printf("INT2_CTRL           0x0D %02x\n\r", dest[i]);
+                break;
+            case 0x0F:
+                debug.printf("WHO_AM_I_XG        0x0F %02x\n\r", dest[i]);
+                break;
+            case 0x10:
+                debug.printf("CTRL_REG1_G         0x10 %02x\n\r", dest[i]);
+                break;
+            case 0x11:
+                debug.printf("CTRL_REG2_G         0x11 %02x\n\r", dest[i]);
+                break;
+            case 0x12:
+                debug.printf("CTRL_REG3_G         0x12 %02x\n\r", dest[i]);
+                break;
+            case 0x13:
+                debug.printf("ORIENT_CFG_G        0x13 %02x\n\r", dest[i]);
+                break;
+            case 0x14:
+                debug.printf("INT_GEN_SRC_G       0x14 %02x\n\r", dest[i]);
+                break;
+            case 0x15:
+                debug.printf("OUT_TEMP_L          0x15 %02x\n\r", dest[i]);
+                break;
+            case 0x16:
+                debug.printf("OUT_TEMP_H          0x16 %02x\n\r", dest[i]);
+                break;
+            case 0x17:
+                debug.printf("STATUS_REG_0        0x17 %02x\n\r", dest[i]);
+                break;
+            case 0x18:
+                debug.printf("OUT_X_L_G           0x18 %02x\n\r", dest[i]);
+                break;
+            case 0x19:
+                debug.printf("OUT_X_H_G           0x19 %02x\n\r", dest[i]);
+                break;
+            case 0x1A:
+                debug.printf("OUT_Y_L_G           0x1A %02x\n\r", dest[i]);
+                break;
+            case 0x1B:
+                debug.printf("OUT_Y_H_G           0x1B %02x\n\r", dest[i]);
+                break;
+            case 0x1C:
+                debug.printf("OUT_Z_L_G           0x1C %02x\n\r", dest[i]);
+                break;
+            case 0x1D:
+                debug.printf("OUT_Z_H_G           0x1D %02x\n\r", dest[i]);
+                break;
+            case 0x1E:
+                debug.printf("CTRL_REG4           0x1E %02x\n\r", dest[i]);
+                break;
+            case 0x1F:
+                debug.printf("CTRL_REG5_XL        0x1F %02x\n\r", dest[i]);
+                break;
+            case 0x20:
+                debug.printf("CTRL_REG6_XL        0x20 %02x\n\r", dest[i]);
+                break;
+            case 0x21:
+                debug.printf("CTRL_REG7_XL        0x21 %02x\n\r", dest[i]);
+                break;
+            case 0x22:
+                debug.printf("CTRL_REG8           0x22 %02x\n\r", dest[i]);
+                break;
+            case 0x23:
+                debug.printf("CTRL_REG9           0x23 %02x\n\r", dest[i]);
+                break;
+            case 0x24:
+                debug.printf("CTRL_REG10          0x24 %02x\n\r", dest[i]);
+                break;
+            case 0x26:
+                debug.printf("INT_GEN_SRC_XL      0x26 %02x\n\r", dest[i]);
+                break;
+            case 0x27:
+                debug.printf("STATUS_REG_1        0x27 %02x\n\r", dest[i]);
+                break;
+            case 0x28:
+                debug.printf("OUT_X_L_XL          0x28 %02x\n\r", dest[i]);
+                break;
+            case 0x29:
+                debug.printf("OUT_X_H_XL          0x29 %02x\n\r", dest[i]);
+                break;
+            case 0x2A:
+                debug.printf("OUT_Y_L_XL          0x2A %02x\n\r", dest[i]);
+                break;
+            case 0x2B:
+                debug.printf("OUT_Y_H_XL          0x2B %02x\n\r", dest[i]);
+                break;
+            case 0x2C:
+                debug.printf("OUT_Z_L_XL          0x2C %02x\n\r", dest[i]);
+                break;
+            case 0x2D:
+                debug.printf("OUT_Z_H_XL          0x2D %02x\n\r", dest[i]);
+                break;
+            case 0x2E:
+                debug.printf("FIFO_CTRL           0x2E %02x\n\r", dest[i]);
+                break;
+            case 0x2F:
+                debug.printf("FIFO_SRC            0x2F %02x\n\r", dest[i]);
+                break;
+            case 0x30:
+                debug.printf("INT_GEN_CFG_G       0x30 %02x\n\r", dest[i]);
+                break;
+            case 0x31:
+                debug.printf("INT_GEN_THS_XH_G    0x31 %02x\n\r", dest[i]);
+                break;
+            case 0x32:
+                debug.printf("INT_GEN_THS_XL_G    0x32 %02x\n\r", dest[i]);
+                break;
+            case 0x33:
+                debug.printf("INT_GEN_THS_YH_G    0x33 %02x\n\r", dest[i]);
+                break;
+            case 0x34:
+                debug.printf("INT_GEN_THS_YL_G    0x34 %02x\n\r", dest[i]);
+                break;
+            case 0x35:
+                debug.printf("INT_GEN_THS_ZH_G    0x35 %02x\n\r", dest[i]);
+                break;
+            case 0x36:
+                debug.printf("INT_GEN_THS_ZL_G    0x36 %02x\n\r", dest[i]);
+                break;
+            case 0x37:
+                debug.printf("INT_GEN_DUR_G       0x37 %02x\n\r", dest[i]);
+                break;
+            default:
+                debug.printf("Register Not Valid  0x%02x\n\r");
+                break;
+        }
+    }
+}
+
+
+
+#define DEPTH 64
+#define DEPP2 6
+
+
+ulint mag_vec( int ax, int ay, int az )
+{
+    static int  x[DEPTH];
+    static int  y[DEPTH];
+    static int  z[DEPTH];
+
+    int i;
+    int sx,sy,sz;
+
+    x[0] = ax;
+    y[0] = ay;
+    z[0] = az;
+    sx = 0;
+    sy = 0;
+    sz = 0;
+
+    for( i = 0; i < DEPTH; i++ ) {
+        sx+= x[i];
+        sy+= y[i];
+        sz+= z[i];
+    }
+    sx >>= DEPP2;
+    sy >>= DEPP2;
+    sz >>= DEPP2;
+
+    for( i = 0; i < DEPTH-1; i++ ) {
+        x[i+1] = x[i];
+        y[i+1] = y[i];
+        z[i+1] = z[i];
+    }
+    return( (ulint)(sx*sx + sy*sy + sz*sz) );
+}
+
+
+ulint maxvec( ulint vec, int reset )
+{
+    ulint static max;
+
+    if( reset == 0 )
+        max  = 0;
+
+    if ( vec > max )  {
+        max = vec;
+    }
+    return( max );
+}
+
+ulint minvec( ulint vec, int reset )
+{
+    ulint static min;
+    if ( reset == 0)
+        min  = 0xFFFFFFFF;
+
+    if ( vec < min )  {
+        min = vec;
+    }
+    return( min );
+
+}
+
+