Revision 12:f361ccfc191e, committed 2016-09-29

Comitter:

afmiee

Date:

Thu Sep 29 22:03:48 2016 +0000

Parent:

11:a246c67d44b0

Commit message:

Minor fixes

Changed in this revision

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 );
+
+}
+
+