Minor fixes
Dependencies: LSM9DS1_Library SDFileSystem mbed nrf51_rtc
Fork of LSM303DLHTest by
Diff: main.cpp
- Revision:
- 10:e7ab0493ed9f
- Parent:
- 9:61fcd186ac50
- Child:
- 11:a246c67d44b0
diff -r 61fcd186ac50 -r e7ab0493ed9f main.cpp --- a/main.cpp Thu Jul 21 20:35:36 2016 +0000 +++ b/main.cpp Tue Jul 26 21:18:15 2016 +0000 @@ -7,15 +7,32 @@ #include "LSM9DS1.h" #include "SDFileSystem.h" #include "nrf51_rtc.h" +//#include "utilities.h" #define M_PI 3.14158 -//#define DEBUG 1 +#define DEBUG 1 + +// 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 0x10 // 0x07 +#define INT_GEN_THS_Y_YL_REG 0xFF // 0x08 +#define INT_GEN_THS_Z_XL_REG 0x10 // 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 + +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 @@ -35,43 +52,14 @@ // Global variables int start = 0; int stop = 0; - - -// Generated when button 1 is pressed on rising edge START -void start_smpl() -{ - start = 1; - stop = 0; - dbg2 = 1; -} - -// Generated when button 1 is pressed on rising edge STOP -void stop_smpl() -{ - stop = 1; - start = 0; - dbg2 = 0; -} +time_t seconds; -void parp( int times ) -{ - int i; - for( i = 0; i < times; i++) { - led1 = 0; - wait( 0.05); - led1 = 1; - wait( 0.05); - } - led2 = 1; -} -// Flipped every second -void flip() -{ - led2 = 0; - wait(0.01); - led2 = 1; -} +FILE *fpA; +FILE *fpG; +FILE *fpM; +char filename[80]; +char secs_str[80]; uint8_t I2CreadByte(uint8_t address, uint8_t subAddress) { @@ -97,8 +85,373 @@ } 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() +{ + //lol.readAccel(); + start = 1; + stop = 0; + //dbg2 = 1; +} + +// Generated when button 1 is pressed on rising edge STOP +// Modified to be generated by Interrupt 2 +void stop_smpl() +{ + stop = 1; + start = 0; + //dbg2 = 0; +} + +void parp( int times ) +{ + int i; + for( i = 0; i < times; i++) { + led1 = 0; + wait( 0.05); + led1 = 1; + wait( 0.05); + } + led2 = 1; +} +// Flipped every second +void flip() +{ + led2 = 0; + wait(0.01); + led2 = 1; + if ( !stop ) { + led1 = 0; + wait(0.01); + led1 = 1; + } +} + +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 ) +{ + 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); + if((file_rename("/sd/ACC.csv",filename )) == 0) { + debug.printf("File ACC.csv renamed successfully to %s\n\r", filename); + } else { + debug.printf("Error: unable to rename the file ACC.csv"); + } + sprintf(filename, "/sd/latch9DOFM_%08x.csv",seconds); + if((file_rename("/sd/MAG.csv",filename )) == 0) { + debug.printf("File MAG.csv renamed successfully to %s\n\r", filename); + } else { + debug.printf("Error: unable to rename the file MAG.csv"); + } + sprintf(filename, "/sd/latch9DOFG_%08x.csv",seconds); + if((file_rename("/sd/GYR.csv",filename )) == 0) { + debug.printf("File GYR.csv renamed successfully to %s\n\r", filename); + } else { + debug.printf("Error: unable to rename the file GYR.csv"); + } +} + +int main() +{ + led1= 1; + + struct tm t; + // debug pins + dbg1 = 0; + dbg2 = 0; + + // 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.getStatus(); + //lol.getAccelIntSrc(); + //lol.readAccel(); + //lol.readGyro(); + + 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) +#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 + int1.fall(&start_smpl); + int2.fall(&stop_smpl); + + // Wait for the start Signal generated by the accelerometer interrupt + 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) + 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) + + while(1) { + // Check for button 1 pressed +#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 + // Wait for the start Signal generated by the accelerometer interrupt + //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) + //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) + +#ifdef DEBUG + print_config_int_registers(); +#endif + open_temp_files(); + start = 0; + while(!start) { + led1 = 1; + } + dbg1 = 1; + // 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 + //lol.getStatus(); + // lol.getAccelIntSrc(); + //lol.readAccel(); + //lol.getGyroIntSrc(); + //lol.readGyro(); + + dbg1 = 0; + // Start sampling + //led1 = 0; + //parp(5); +#ifdef DEBUG + debug.printf("Started sampling\n\r"); + // Get the time and create a file with the number of seconds in hex appended +#endif + seconds = rtc.time(); + + sprintf(secs_str, "%s", ctime(&seconds)); +#ifdef DEBUG + debug.printf("\n\rStarted at: %s\n\r\n\r", secs_str ); +#endif + // 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) + // Execute until inctivity timer is triggered +#ifdef DEBUG + print_config_int_registers(); +#endif + while(!stop) { + dbg2 = 1; + 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); + } + } + dbg2 = 0; + //lol.configInt(XG_INT2, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT2_CTRL (0x0D) + CTRL_REG8 (0x22) + + // Stop Sampling and close file + parp(10); +#ifdef DEBUG + debug.printf("Stopped sampling\n\r"); +#endif + fclose(fpA); + fclose(fpM); + fclose(fpG); + rename_files(); + } +} void DumpAccelGyroRegs( void ) { @@ -268,299 +621,70 @@ } } -//*********************************************************** -// 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); -} - -int main() -{ - led1= 1; - char filename[256]; - char secs_str[256]; - - struct tm t; - time_t seconds; - - FILE *fpA; - FILE *fpG; - FILE *fpM; - FILE *test; - // debug pins - dbg1 = 0; - dbg2 = 0; - - // 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 - //lol.begin(); - 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", filename); - } - - lol.calibrate(); - - lol.configInt(XG_INT1, INT1_IG_G, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22) - lol.configInt(XG_INT2, INT2_INACT, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT2_CTRL (0x0D) + CTRL_REG8 (0x22) - - lol.configAccelInt(ZHIE_XL|YHIE_XL|XHIE_XL, false); // INT_GEN_CFG_XL (06h) - lol.configAccelThs((uint8_t)0x25, X_AXIS, (uint8_t)0x00, false); // INT_GEN_THS_X_XL (07h) - lol.configAccelThs((uint8_t)0x25, Y_AXIS, (uint8_t)0x00, false); // INT_GEN_THS_Y_XL (08h) - lol.configAccelThs((uint8_t)0x25, Z_AXIS, (uint8_t)0x00, false); // INT_GEN_THS_Z_XL (09h) - - lol.configGyroInt(ZHIE_G|YHIE_G|XHIE_G, false, false); // INT_GEN_CFG_G (30h) - lol.configGyroThs((int16_t )0x300, X_AXIS, (uint8_t) 0x02, false); // INT_GEN_THS_X_G (31h - 32h) - lol.configGyroThs((int16_t )0x300, Y_AXIS, (uint8_t) 0x02, false); // INT_GEN_THS_Y_G (33h - 34h) - lol.configGyroThs((int16_t )0x300, Z_AXIS, (uint8_t) 0x02, false); // INT_GEN_THS_Z_G (35h - 36h) - - lol.configInactivity(0x0F, 0x20, true); - -#ifdef DEBUG - debug.printf( "\n\r"); - 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( "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)); - -// 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); -#endif -// 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)); - int1.fall(&start_smpl); - int2.fall(&stop_smpl); - - while(1) { - //debug.printf("Press Button 1 to Start sampling\n\r"); - //debug.printf("Press Button 2 to Stop sampling\n\r"); - // Check for button 1 pressed - while(!start) { - led1 = 1; - } - dbg2 = 0; - // Start sampling - //led1 = 0; - parp(5); -//#ifdef DEBUG - debug.printf("Started sampling\n\r"); - // Get the time and create a file with the number of seconds in hex appended -//#endif - seconds = rtc.time(); - dbg1 = 1; - sprintf(secs_str, "%s", ctime(&seconds)); - printf("\n\rStarted at: %s\n\r\n\r", secs_str ); - sprintf(filename, "/sd/latch9DOFA_%08x.csv",seconds); - - test = fopen("/sd/test_original.csv", "w"); - // Verify that file can be created - if ( test == NULL ) { - debug.printf("Cannot create file test_original.csv\n\r"); - wait(0.5); - while(1) { - led1 = !led1; - wait(0.5); - } - } else - debug.printf("File test_original.csv created successfully\n\r", filename); +#define DEPTH 64 +#define DEPP2 6 - fpA = fopen(filename, "w"); - // Verify that file can be created - if ( fpA == NULL ) { - debug.printf("Cannot create file %s\n\r", filename); - wait(0.5); - while(1) { - led1 = !led1; - wait(0.5); - } - } else - debug.printf("File %s created successfully\n\r", filename); +ulint mag_vec( int ax, int ay, int az ) +{ + static int x[DEPTH]; + static int y[DEPTH]; + static int z[DEPTH]; - sprintf(filename, "/sd/latch9DOFG_%08x.csv",seconds); - fpG = fopen(filename, "w"); - // Verify that file can be created - if ( fpG == NULL ) { - debug.printf("Cannot create file %s\n\r", filename); - wait(0.5); - while(1) { - led1 = !led1; - wait(0.5); - } - } else - debug.printf("File %s created successfully\n\r", filename); + int i; + int sx,sy,sz; + + x[0] = ax; + y[0] = ay; + z[0] = az; + sx = 0; + sy = 0; + sz = 0; - sprintf(filename, "/sd/latch9DOFM_%08x.csv",seconds); - fpM = fopen(filename, "w"); - // Verify that file can be created - if ( fpM == NULL ) { - debug.printf("Cannot create file %s\n\r", filename); - wait(0.5); - while(1) { - led1 = !led1; - wait(0.5); - } - } else - debug.printf("File %s created successfully\n\r", filename); - dbg1 = 0; - // Sample until button 2 is pressed - while(!stop) { - //led1 = 0; - //lol.getAccelIntSrc(); // INT_GEN_SRC_XL (26h) - //I2CreadByte(0xD6, 0x27); - 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); - } - //lol.getMagIntSrc(); // INT_SRC_M (31h) - //I2CreadByte(0xD6, 0x27); - if ( lol.magAvailable(X_AXIS) && lol.magAvailable(X_AXIS) && lol.magAvailable(X_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); - } - //lol.getGyroIntSrc(); // INT_GEN_SRC_G (14h) - 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); - } - //wait(0.1); - } - // Stop Sampling and close file - parp(10); -//#ifdef DEBUG - debug.printf("Stopped sampling\n\r"); - //#endif - fclose(fpA); - fclose(fpM); - fclose(fpG); - fclose(test); + for( i = 0; i < DEPTH; i++ ) { + sx+= x[i]; + sy+= y[i]; + sz+= z[i]; + } + sx >>= DEPP2; + sy >>= DEPP2; + sz >>= DEPP2; - int ret; - - ret = file_rename("/sd/test_original.csv","/sd/test_renamed.csv"); - - if(ret == 0) { - debug.printf("File renamed successfully"); - } else { - debug.printf("Error: unable to rename the file"); - } + 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 ); + +} + +