Antonio Mondragon
Minor fixes
Dependencies: LSM9DS1_Library SDFileSystem mbed nrf51_rtc
Fork of
LSM303DLHTest
by 
Toshihisa T
Diff: main.cpp
- Revision:
- 10:e7ab0493ed9f
- Parent:
- 9:61fcd186ac50
- Child:
- 11:a246c67d44b0
--- 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 );
+
+}
+
+