Antonio Mondragon / Mbed 2 deprecated Latch9DOF_LSM9DS1

Minor fixes

Dependencies:   LSM9DS1_Library SDFileSystem mbed nrf51_rtc

Fork of LSM303DLHTest by Toshihisa T

temp.txt@12:f361ccfc191e, 2016-09-29 (annotated)

Committer:
afmiee
Date:
Thu Sep 29 22:03:48 2016 +0000
Revision:
12:f361ccfc191e
Parent:
11:a246c67d44b0 
Minor fixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
afmiee 12:f361ccfc191e 1 // Latch Inc.
afmiee 12:f361ccfc191e 2 // Antonio F Mondragon
afmiee 12:f361ccfc191e 3 // 20160714
afmiee 12:f361ccfc191e 4 // for the Adafruit 9DOF Modulke and the Sparkfun microSD card shield
afmiee 12:f361ccfc191e 5
afmiee 12:f361ccfc191e 6 #include "mbed.h"
afmiee 12:f361ccfc191e 7 #include "LSM9DS1.h"
afmiee 12:f361ccfc191e 8 #include "SDFileSystem.h"
afmiee 12:f361ccfc191e 9 #include "nrf51_rtc.h"
afmiee 12:f361ccfc191e 10
afmiee 12:f361ccfc191e 11 #define M_PI 3.14158
afmiee 12:f361ccfc191e 12 //#define DEBUG 1 // Print Debug information to serial terminal
afmiee 12:f361ccfc191e 13 //#define MOTION 1 // Define if operated by motion or by buttons
afmiee 12:f361ccfc191e 14
afmiee 12:f361ccfc191e 15 // define threshold and Duration registers for interrupts
afmiee 12:f361ccfc191e 16 #define ACT_THS_REG 0x10 // 0x04
afmiee 12:f361ccfc191e 17 #define ACT_DUR_REG 0x02 // 0x05
afmiee 12:f361ccfc191e 18
afmiee 12:f361ccfc191e 19 #define INT_GEN_THS_X_XL_REG 0x0C // 0x07
afmiee 12:f361ccfc191e 20 #define INT_GEN_THS_Y_YL_REG 0xFF // 0x08
afmiee 12:f361ccfc191e 21 #define INT_GEN_THS_Z_XL_REG 0x0C // 0x09
afmiee 12:f361ccfc191e 22 #define INT_GEN_DUR_XL_REG 0x00 // 0x0A
afmiee 12:f361ccfc191e 23
afmiee 12:f361ccfc191e 24 #define INT_GEN_THS_XHL_G_REG 0x0300 // 0x31-32
afmiee 12:f361ccfc191e 25 #define INT_GEN_THS_YHL_G_REG 0x0300 // 0x33-34
afmiee 12:f361ccfc191e 26 #define INT_GEN_THS_ZHL_G_REG 0x0300 // 0x35-36
afmiee 12:f361ccfc191e 27 #define INT_GEN_DUR_G_REG 0x02 // 0x37
afmiee 12:f361ccfc191e 28
afmiee 12:f361ccfc191e 29 #define LED_ON 0
afmiee 12:f361ccfc191e 30 #define LED_OFF 01
afmiee 12:f361ccfc191e 31 #define DBG_ACTIVE 0
afmiee 12:f361ccfc191e 32 #define DBG_INACTIVE 1
afmiee 12:f361ccfc191e 33
afmiee 12:f361ccfc191e 34 typedef unsigned long int ulint;
afmiee 12:f361ccfc191e 35
afmiee 12:f361ccfc191e 36 // Create objects
afmiee 12:f361ccfc191e 37 Serial debug(USBTX,USBRX);
afmiee 12:f361ccfc191e 38 // For Nordic
afmiee 12:f361ccfc191e 39 LSM9DS1 lol(p30, p7, 0xD6, 0x3C);
afmiee 12:f361ccfc191e 40 I2C i2c(p30, p7);
afmiee 12:f361ccfc191e 41
afmiee 12:f361ccfc191e 42 // Create the SD filesystem
afmiee 12:f361ccfc191e 43 SDFileSystem sd(p25, p28, p29, p20, "sd"); // MOSI, MISO, SCLK, SSEL
afmiee 12:f361ccfc191e 44
afmiee 12:f361ccfc191e 45 // Create a ticker to use the nRF51 RTC
afmiee 12:f361ccfc191e 46 Ticker flipper;
afmiee 12:f361ccfc191e 47
afmiee 12:f361ccfc191e 48 #ifdef MOTION
afmiee 12:f361ccfc191e 49 // Assign interrupts to Interrupts from LSM9DS1
afmiee 12:f361ccfc191e 50 InterruptIn int1(p12); // Start sampling
afmiee 12:f361ccfc191e 51 InterruptIn int2(p13); // Stop sampling
afmiee 12:f361ccfc191e 52 #else
afmiee 12:f361ccfc191e 53 // Assign interrupts to Buttons 1 and 2 on nrf51-DK
afmiee 12:f361ccfc191e 54 InterruptIn but1(p17); // Start sampling
afmiee 12:f361ccfc191e 55 InterruptIn but2(p18); // Stop sampling
afmiee 12:f361ccfc191e 56 #endif
afmiee 12:f361ccfc191e 57
afmiee 12:f361ccfc191e 58 // LED definitions
afmiee 12:f361ccfc191e 59 DigitalOut led1(LED1);
afmiee 12:f361ccfc191e 60 DigitalOut led2(LED2);
afmiee 12:f361ccfc191e 61 DigitalOut led3(LED3);
afmiee 12:f361ccfc191e 62
afmiee 12:f361ccfc191e 63 DigitalOut dbg1(p24);
afmiee 12:f361ccfc191e 64 DigitalOut dbg2(p0);
afmiee 12:f361ccfc191e 65
afmiee 12:f361ccfc191e 66 // Global variables
afmiee 12:f361ccfc191e 67 volatile int start = 0;
afmiee 12:f361ccfc191e 68 volatile int stop = 0;
afmiee 12:f361ccfc191e 69 time_t seconds;
afmiee 12:f361ccfc191e 70
afmiee 12:f361ccfc191e 71 FILE *fpA;
afmiee 12:f361ccfc191e 72 FILE *fpG;
afmiee 12:f361ccfc191e 73 FILE *fpM;
afmiee 12:f361ccfc191e 74
afmiee 12:f361ccfc191e 75 char filename[80];
afmiee 12:f361ccfc191e 76 char secs_str[80];
afmiee 12:f361ccfc191e 77
afmiee 11:a246c67d44b0 78 uint8_t I2CreadByte(uint8_t address, uint8_t subAddress);
afmiee 11:a246c67d44b0 79 uint8_t I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count);
afmiee 11:a246c67d44b0 80 int file_rename(const char *oldfname, const char *newfname);
afmiee 11:a246c67d44b0 81 int file_copy (const char *src, const char *dst);
afmiee 11:a246c67d44b0 82 void start_smpl();
afmiee 11:a246c67d44b0 83 void stop_smpl();
afmiee 11:a246c67d44b0 84 void parp( int times );
afmiee 11:a246c67d44b0 85 void flip();
afmiee 11:a246c67d44b0 86 void print_config_int_registers( void );
afmiee 11:a246c67d44b0 87 void open_temp_files( void );
afmiee 11:a246c67d44b0 88 void rename_files( void );
afmiee 11:a246c67d44b0 89 void DumpAccelGyroRegs( void );
afmiee 11:a246c67d44b0 90
afmiee 11:a246c67d44b0 91
afmiee 12:f361ccfc191e 92 int main()
afmiee 12:f361ccfc191e 93 {
afmiee 12:f361ccfc191e 94 led1 = 1;
afmiee 12:f361ccfc191e 95 led2 = 1;
afmiee 12:f361ccfc191e 96
afmiee 12:f361ccfc191e 97 // debug pins
afmiee 12:f361ccfc191e 98 dbg1 = DBG_INACTIVE;
afmiee 12:f361ccfc191e 99 dbg2 = DBG_INACTIVE;
afmiee 12:f361ccfc191e 100
afmiee 12:f361ccfc191e 101 struct tm t;
afmiee 12:f361ccfc191e 102 start = 0;
afmiee 12:f361ccfc191e 103 stop = 0;
afmiee 12:f361ccfc191e 104
afmiee 12:f361ccfc191e 105 #ifdef MOTION
afmiee 12:f361ccfc191e 106 int1.fall(&start_smpl);
afmiee 12:f361ccfc191e 107 int2.fall(&stop_smpl);
afmiee 12:f361ccfc191e 108 #else
afmiee 12:f361ccfc191e 109 but1.rise(&start_smpl);
afmiee 12:f361ccfc191e 110 but2.rise(&stop_smpl);
afmiee 12:f361ccfc191e 111 #endif
afmiee 12:f361ccfc191e 112
afmiee 12:f361ccfc191e 113 // Attach functions to interrupts
afmiee 12:f361ccfc191e 114 flipper.attach(&flip, 1.0); // the address of the function to be attached (flip) and the interval (2 seconds)
afmiee 12:f361ccfc191e 115
afmiee 12:f361ccfc191e 116 // Enable serial port
afmiee 12:f361ccfc191e 117 debug.format(8,Serial::None,1);
afmiee 12:f361ccfc191e 118 debug.baud(115200);
afmiee 12:f361ccfc191e 119
afmiee 12:f361ccfc191e 120 #ifdef DEBUG
afmiee 12:f361ccfc191e 121 debug.printf("LSM9DS1 Test\x0d\x0a");
afmiee 12:f361ccfc191e 122 #endif
afmiee 12:f361ccfc191e 123
afmiee 12:f361ccfc191e 124 // Initialize 9DOF
afmiee 12:f361ccfc191e 125 if (!lol.begin()) {
afmiee 12:f361ccfc191e 126 debug.printf("Failed to communicate with LSM9DS1.\n");
afmiee 12:f361ccfc191e 127 wait(0.5);
afmiee 12:f361ccfc191e 128 while(1) {
afmiee 12:f361ccfc191e 129 led1 = !led1;
afmiee 12:f361ccfc191e 130 wait(0.5);
afmiee 12:f361ccfc191e 131 }
afmiee 12:f361ccfc191e 132 } else {
afmiee 12:f361ccfc191e 133 debug.printf("Communication with the LSM9DS1 successfully\n\r");
afmiee 12:f361ccfc191e 134 }
afmiee 12:f361ccfc191e 135
afmiee 12:f361ccfc191e 136 lol.calibrate(true);
afmiee 12:f361ccfc191e 137 lol.setFIFO(FIFO_CONT, 0x1F);
afmiee 12:f361ccfc191e 138
afmiee 12:f361ccfc191e 139 lol.getStatus();
afmiee 12:f361ccfc191e 140 lol.getAccelIntSrc();
afmiee 12:f361ccfc191e 141 lol.readAccel();
afmiee 12:f361ccfc191e 142 lol.readGyro();
afmiee 12:f361ccfc191e 143
afmiee 12:f361ccfc191e 144 #ifdef MOTION
afmiee 12:f361ccfc191e 145 // Configure
afmiee 12:f361ccfc191e 146 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)
afmiee 12:f361ccfc191e 147 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)
afmiee 12:f361ccfc191e 148 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)
afmiee 12:f361ccfc191e 149
afmiee 12:f361ccfc191e 150 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)
afmiee 12:f361ccfc191e 151 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)
afmiee 12:f361ccfc191e 152 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)
afmiee 12:f361ccfc191e 153 //lol.configGyroInt(ZHIE_G|YHIE_G|XHIE_G, false, false); // INT_GEN_CFG_G (30h)
afmiee 12:f361ccfc191e 154
afmiee 12:f361ccfc191e 155 lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 156 lol.configInt(XG_INT2, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 157 #endif
afmiee 12:f361ccfc191e 158
afmiee 12:f361ccfc191e 159 #ifdef DEBUG
afmiee 12:f361ccfc191e 160 print_config_int_registers();
afmiee 12:f361ccfc191e 161 #endif
afmiee 12:f361ccfc191e 162 // Dump all registers
afmiee 12:f361ccfc191e 163 //DumpAccelGyroRegs();
afmiee 12:f361ccfc191e 164 // // Initialize current time if needed
afmiee 12:f361ccfc191e 165 // printf("Enter current date and time:\n");
afmiee 12:f361ccfc191e 166 // printf("YYYY MM DD HH MM SS[enter]\n");
afmiee 12:f361ccfc191e 167 // scanf("%d %d %d %d %d %d", &t.tm_year, &t.tm_mon, &t.tm_mday
afmiee 12:f361ccfc191e 168 // , &t.tm_hour, &t.tm_min, &t.tm_sec);
afmiee 12:f361ccfc191e 169
afmiee 12:f361ccfc191e 170 // adjust for tm structure required values
afmiee 12:f361ccfc191e 171 t.tm_year = t.tm_year - 1900;
afmiee 12:f361ccfc191e 172 t.tm_mon = t.tm_mon - 1;
afmiee 12:f361ccfc191e 173
afmiee 12:f361ccfc191e 174 // set the time
afmiee 12:f361ccfc191e 175 rtc.set_time(mktime(&t));
afmiee 12:f361ccfc191e 176 // Set the interrupt service routines
afmiee 12:f361ccfc191e 177
afmiee 12:f361ccfc191e 178 // Wait for the start Signal generated by the accelerometer interrupt
afmiee 12:f361ccfc191e 179 #ifdef MOTION
afmiee 12:f361ccfc191e 180 // Read the interrupt to clear it
afmiee 12:f361ccfc191e 181 lol.getStatus();
afmiee 12:f361ccfc191e 182 lol.getAccelIntSrc();
afmiee 12:f361ccfc191e 183 // Disable the interrupt
afmiee 12:f361ccfc191e 184 lol.configAccelInt(0, false); // INT_GEN_CFG_XL (06h)
afmiee 12:f361ccfc191e 185 lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 186 #endif
afmiee 11:a246c67d44b0 187
afmiee 11:a246c67d44b0 188
afmiee 12:f361ccfc191e 189 while(1) {
afmiee 12:f361ccfc191e 190 #ifdef MOTION
afmiee 12:f361ccfc191e 191 lol.getStatus();
afmiee 12:f361ccfc191e 192 lol.getAccelIntSrc();
afmiee 12:f361ccfc191e 193 lol.readAccel();
afmiee 12:f361ccfc191e 194 lol.getGyroIntSrc();
afmiee 12:f361ccfc191e 195 lol.readGyro();
afmiee 12:f361ccfc191e 196 // Disable the interrupt
afmiee 12:f361ccfc191e 197 lol.configAccelInt(0, false); // INT_GEN_CFG_XL (06h)
afmiee 12:f361ccfc191e 198 lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 199 // Read the interrupt to clear it
afmiee 12:f361ccfc191e 200 #endif
afmiee 12:f361ccfc191e 201 // Create temporary files
afmiee 12:f361ccfc191e 202 led3 = LED_ON;
afmiee 12:f361ccfc191e 203 open_temp_files();
afmiee 12:f361ccfc191e 204 led3 = LED_OFF;
afmiee 12:f361ccfc191e 205 #ifdef DEBUG
afmiee 12:f361ccfc191e 206 debug.printf( "\n\r");
afmiee 12:f361ccfc191e 207 debug.printf( "Status (27h) %02x\n\r", lol.getStatus()); // STATUS_REG (27h)
afmiee 12:f361ccfc191e 208 debug.printf( "GyroIntSrc (14h) %02x\n\r", lol.getGyroIntSrc()); // INT_GEN_SRC_G (14h)
afmiee 12:f361ccfc191e 209 debug.printf( "AccelIntSrc(26h) %02x\n\r", lol.getAccelIntSrc()); // INT_GEN_SRC_XL (26h)
afmiee 12:f361ccfc191e 210 debug.printf( "MagIntSrc (31h) %02x\n\r", lol.getMagIntSrc()); // INT_SRC_M (31h)
afmiee 12:f361ccfc191e 211 debug.printf( "Inactivity (17h) %02x\n\r", lol.getInactivity()); // STATUS_REG (17h)
afmiee 12:f361ccfc191e 212 debug.printf( "\n\r");
afmiee 12:f361ccfc191e 213 #endif
afmiee 12:f361ccfc191e 214 #ifdef DEBUG
afmiee 12:f361ccfc191e 215 print_config_int_registers();
afmiee 12:f361ccfc191e 216 #endif
afmiee 12:f361ccfc191e 217 #ifdef MOTION
afmiee 12:f361ccfc191e 218 // Program the motion interrupt on accelerometer
afmiee 12:f361ccfc191e 219 lol.configAccelInt(ZHIE_XL|YHIE_XL|XHIE_XL, false); // INT_GEN_CFG_XL (06h)
afmiee 12:f361ccfc191e 220 lol.configInt(XG_INT1, INT1_IG_XL, INT_ACTIVE_LOW, INT_PUSH_PULL); // INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 221 #endif
afmiee 12:f361ccfc191e 222 // Check for button 1 pressed or
afmiee 12:f361ccfc191e 223 // Wait for the start Signal generated by the accelerometer interrupt
afmiee 12:f361ccfc191e 224 // Depends if MOTION is defined
afmiee 12:f361ccfc191e 225 #ifdef DEBUG
afmiee 12:f361ccfc191e 226 #ifdef MOTION
afmiee 12:f361ccfc191e 227 debug.printf("Waiting for motion to start sampling\n\r");
afmiee 12:f361ccfc191e 228 #else
afmiee 12:f361ccfc191e 229 debug.printf("Waiting for Button 1 to be pressed to start sampling\n\r");
afmiee 12:f361ccfc191e 230 #endif
afmiee 12:f361ccfc191e 231 #endif
afmiee 12:f361ccfc191e 232 while(start==0);
afmiee 12:f361ccfc191e 233 dbg1 = DBG_ACTIVE;
afmiee 12:f361ccfc191e 234 dbg2 = DBG_INACTIVE;
afmiee 12:f361ccfc191e 235 #ifdef DEBUG
afmiee 12:f361ccfc191e 236 #ifdef MOTION
afmiee 12:f361ccfc191e 237 debug.printf("Motion Detected\n\r");
afmiee 12:f361ccfc191e 238 #else
afmiee 12:f361ccfc191e 239 debug.printf("Button 1 pressed\n\r");
afmiee 12:f361ccfc191e 240 #endif
afmiee 12:f361ccfc191e 241 #endif
afmiee 12:f361ccfc191e 242 #ifdef MOTION
afmiee 12:f361ccfc191e 243 // Reset the Interrupt
afmiee 12:f361ccfc191e 244 lol.getStatus();
afmiee 12:f361ccfc191e 245 lol.getAccelIntSrc();
afmiee 12:f361ccfc191e 246
afmiee 12:f361ccfc191e 247 // Disable the interrupt
afmiee 12:f361ccfc191e 248 lol.configAccelInt(0, false); // INT_GEN_CFG_XL (06h)
afmiee 12:f361ccfc191e 249 lol.configInt(XG_INT1, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT1_CTRL (0x0C) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 250 // Read the interrupt to clear it
afmiee 12:f361ccfc191e 251 #endif
afmiee 12:f361ccfc191e 252 #ifdef DEBUG
afmiee 12:f361ccfc191e 253 debug.printf("Started sampling\n\r");
afmiee 12:f361ccfc191e 254 // Get the time and create a file with the number of seconds in hex appended
afmiee 12:f361ccfc191e 255 #endif
afmiee 12:f361ccfc191e 256 // Caoture the seconds since Turn-on to name the files
afmiee 12:f361ccfc191e 257 seconds = rtc.time();
afmiee 12:f361ccfc191e 258 sprintf(secs_str, "%s", ctime(&seconds));
afmiee 12:f361ccfc191e 259 #ifdef DEBUG
afmiee 12:f361ccfc191e 260 debug.printf("\n\rStarted at: %s\n\r\n\r", secs_str );
afmiee 12:f361ccfc191e 261 #endif
afmiee 12:f361ccfc191e 262 #ifdef MOTION
afmiee 12:f361ccfc191e 263 // program inactivity timer
afmiee 12:f361ccfc191e 264 lol.configInactivity(ACT_THS_REG, ACT_DUR_REG, true);
afmiee 12:f361ccfc191e 265 lol.configInt(XG_INT2, INT2_INACT, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT2_CTRL (0x0D) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 266 #endif
afmiee 12:f361ccfc191e 267 //Execute until inactivity timer is triggered
afmiee 12:f361ccfc191e 268 #ifdef DEBUG
afmiee 12:f361ccfc191e 269 print_config_int_registers();
afmiee 12:f361ccfc191e 270 #endif
afmiee 12:f361ccfc191e 271
afmiee 12:f361ccfc191e 272 while(!stop) {
afmiee 12:f361ccfc191e 273 dbg1 = DBG_INACTIVE;
afmiee 12:f361ccfc191e 274 dbg2 = DBG_ACTIVE;
afmiee 12:f361ccfc191e 275 if (lol.accelAvailable()) {
afmiee 12:f361ccfc191e 276 lol.readAccel();
afmiee 12:f361ccfc191e 277 #ifdef DEBUG
afmiee 12:f361ccfc191e 278 debug.printf("ACC %d, %d, %d\n\r", lol.ax, lol.ay, lol.az);
afmiee 12:f361ccfc191e 279 #endif
afmiee 12:f361ccfc191e 280 fprintf(fpA, "%d, %d, %d\n\r", lol.ax, lol.ay, lol.az);
afmiee 12:f361ccfc191e 281 }
afmiee 12:f361ccfc191e 282 if ( lol.magAvailable(X_AXIS) && lol.magAvailable(Y_AXIS) && lol.magAvailable(Z_AXIS)) {
afmiee 12:f361ccfc191e 283 lol.readMag();
afmiee 12:f361ccfc191e 284 #ifdef DEBUG
afmiee 12:f361ccfc191e 285 debug.printf("MAG %d, %d, %d\n\r", lol.mx, lol.my, lol.mz);
afmiee 12:f361ccfc191e 286 #endif
afmiee 12:f361ccfc191e 287 fprintf(fpM, "%d, %d, %d\n\r", lol.mx, lol.my, lol.mz);
afmiee 12:f361ccfc191e 288 }
afmiee 12:f361ccfc191e 289 if ( lol.gyroAvailable()) {
afmiee 12:f361ccfc191e 290 lol.readGyro();
afmiee 12:f361ccfc191e 291 #ifdef DEBUG
afmiee 12:f361ccfc191e 292 debug.printf("GYR %d, %d, %d\n\r", lol.gx, lol.gy, lol.gz);
afmiee 12:f361ccfc191e 293 #endif
afmiee 12:f361ccfc191e 294 fprintf(fpG, "%d, %d, %d\n\r", lol.gx, lol.gy, lol.gz);
afmiee 12:f361ccfc191e 295 }
afmiee 12:f361ccfc191e 296 }
afmiee 12:f361ccfc191e 297 #ifdef MOTION
afmiee 12:f361ccfc191e 298 //Disable inactivity interrupt
afmiee 12:f361ccfc191e 299 lol.configInt(XG_INT2, 0, INT_ACTIVE_LOW, INT_PUSH_PULL); //INT2_CTRL (0x0D) + CTRL_REG8 (0x22)
afmiee 12:f361ccfc191e 300 #endif
afmiee 12:f361ccfc191e 301 // Stop Sampling and close file
afmiee 12:f361ccfc191e 302 //parp(10);
afmiee 12:f361ccfc191e 303 #ifdef DEBUG
afmiee 12:f361ccfc191e 304 debug.printf("Stopped sampling\n\r");
afmiee 12:f361ccfc191e 305 #endif
afmiee 12:f361ccfc191e 306 led3 = LED_ON;
afmiee 12:f361ccfc191e 307 fclose(fpA);
afmiee 12:f361ccfc191e 308 fclose(fpM);
afmiee 12:f361ccfc191e 309 fclose(fpG);
afmiee 12:f361ccfc191e 310 rename_files();
afmiee 12:f361ccfc191e 311 led3 = LED_OFF;
afmiee 12:f361ccfc191e 312 }
afmiee 12:f361ccfc191e 313 }
afmiee 12:f361ccfc191e 314
afmiee 12:f361ccfc191e 315 uint8_t I2CreadByte(uint8_t address, uint8_t subAddress)
afmiee 12:f361ccfc191e 316 {
afmiee 12:f361ccfc191e 317 char data;
afmiee 12:f361ccfc191e 318 char temp= subAddress;
afmiee 12:f361ccfc191e 319
afmiee 12:f361ccfc191e 320 i2c.write(address, &temp, 1);
afmiee 12:f361ccfc191e 321 int a = i2c.read(address, &data, 1);
afmiee 12:f361ccfc191e 322 return data;
afmiee 12:f361ccfc191e 323 }
afmiee 12:f361ccfc191e 324
afmiee 12:f361ccfc191e 325 uint8_t I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
afmiee 12:f361ccfc191e 326 {
afmiee 12:f361ccfc191e 327 int i;
afmiee 12:f361ccfc191e 328 char temp_dest[count];
afmiee 12:f361ccfc191e 329 char temp = subAddress;
afmiee 12:f361ccfc191e 330 i2c.write(address, &temp, 1);
afmiee 12:f361ccfc191e 331 i2c.read(address, temp_dest, count);
afmiee 12:f361ccfc191e 332
afmiee 12:f361ccfc191e 333 //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion
afmiee 12:f361ccfc191e 334 for (i=0; i < count; i++) {
afmiee 12:f361ccfc191e 335 dest[i] = temp_dest[i];
afmiee 12:f361ccfc191e 336 }
afmiee 12:f361ccfc191e 337 return count;
afmiee 12:f361ccfc191e 338 }
afmiee 12:f361ccfc191e 339 //***********************************************************
afmiee 12:f361ccfc191e 340 // file_rename: renames a file (via copy & delete).
afmiee 12:f361ccfc191e 341 // Moves data instead of adjusting the file name in the
afmiee 12:f361ccfc191e 342 // file directory. Checks to insure the file was renamed.
afmiee 12:f361ccfc191e 343 // Returns -1 = error; 0 = success
afmiee 12:f361ccfc191e 344 //***********************************************************
afmiee 12:f361ccfc191e 345 int file_rename(const char *oldfname, const char *newfname)
afmiee 12:f361ccfc191e 346 {
afmiee 12:f361ccfc191e 347 int retval = 0;
afmiee 12:f361ccfc191e 348 int ch;
afmiee 12:f361ccfc191e 349
afmiee 12:f361ccfc191e 350 FILE *fpold = fopen(oldfname, "r"); // src file
afmiee 12:f361ccfc191e 351 FILE *fpnew = fopen(newfname, "w"); // dest file
afmiee 12:f361ccfc191e 352
afmiee 12:f361ccfc191e 353 while (1) { // Copy src to dest
afmiee 12:f361ccfc191e 354 ch = fgetc(fpold); // until src EOF read.
afmiee 12:f361ccfc191e 355 if (ch == EOF) break;
afmiee 12:f361ccfc191e 356 fputc(ch, fpnew);
afmiee 12:f361ccfc191e 357 }
afmiee 12:f361ccfc191e 358
afmiee 12:f361ccfc191e 359 fclose(fpnew);
afmiee 12:f361ccfc191e 360 fclose(fpold);
afmiee 12:f361ccfc191e 361
afmiee 12:f361ccfc191e 362 fpnew = fopen(newfname, "r"); // Reopen dest to insure
afmiee 12:f361ccfc191e 363 if(fpnew == NULL) { // that it was created.
afmiee 12:f361ccfc191e 364 retval = (-1); // Return Error.
afmiee 12:f361ccfc191e 365 } else {
afmiee 12:f361ccfc191e 366 fclose(fpnew);
afmiee 12:f361ccfc191e 367 remove(oldfname); // Remove original file.
afmiee 12:f361ccfc191e 368 retval = (0); // Return Success.
afmiee 12:f361ccfc191e 369 }
afmiee 12:f361ccfc191e 370 return (retval);
afmiee 12:f361ccfc191e 371 }
afmiee 12:f361ccfc191e 372
afmiee 12:f361ccfc191e 373 //***********************************************************
afmiee 12:f361ccfc191e 374 // file_copy: Copies a file
afmiee 12:f361ccfc191e 375 // Checks to insure destination file was created.
afmiee 12:f361ccfc191e 376 // Returns -1 = error; 0 = success
afmiee 12:f361ccfc191e 377 //***********************************************************
afmiee 12:f361ccfc191e 378 int file_copy (const char *src, const char *dst)
afmiee 12:f361ccfc191e 379 {
afmiee 12:f361ccfc191e 380 int retval = 0;
afmiee 12:f361ccfc191e 381 int ch;
afmiee 12:f361ccfc191e 382
afmiee 12:f361ccfc191e 383 FILE *fpsrc = fopen(src, "r"); // src file
afmiee 12:f361ccfc191e 384 FILE *fpdst = fopen(dst, "w"); // dest file
afmiee 12:f361ccfc191e 385
afmiee 12:f361ccfc191e 386 while (1) { // Copy src to dest
afmiee 12:f361ccfc191e 387 ch = fgetc(fpsrc); // until src EOF read.
afmiee 12:f361ccfc191e 388 if (ch == EOF) break;
afmiee 12:f361ccfc191e 389 fputc(ch, fpdst);
afmiee 12:f361ccfc191e 390 }
afmiee 12:f361ccfc191e 391 fclose(fpsrc);
afmiee 12:f361ccfc191e 392 fclose(fpdst);
afmiee 12:f361ccfc191e 393
afmiee 12:f361ccfc191e 394 fpdst = fopen(dst, "r"); // Reopen dest to insure
afmiee 12:f361ccfc191e 395 if(fpdst == NULL) { // that it was created.
afmiee 12:f361ccfc191e 396 retval = (-1); // Return error.
afmiee 12:f361ccfc191e 397 } else {
afmiee 12:f361ccfc191e 398 fclose(fpdst);
afmiee 12:f361ccfc191e 399 retval = (0); // Return success.
afmiee 12:f361ccfc191e 400 }
afmiee 12:f361ccfc191e 401 return (retval);
afmiee 12:f361ccfc191e 402 }
afmiee 12:f361ccfc191e 403 // Generated when button 1 is pressed on rising edge START
afmiee 12:f361ccfc191e 404 // Modified to be generated by Interrupt 1
afmiee 12:f361ccfc191e 405 void start_smpl()
afmiee 12:f361ccfc191e 406 {
afmiee 12:f361ccfc191e 407 start = 1;
afmiee 12:f361ccfc191e 408 stop = 0;
afmiee 12:f361ccfc191e 409 //dbg1 = 1;
afmiee 12:f361ccfc191e 410 //dbg2 = 0;
afmiee 12:f361ccfc191e 411 }
afmiee 12:f361ccfc191e 412
afmiee 12:f361ccfc191e 413 // Generated when button 1 is pressed on rising edge STOP
afmiee 12:f361ccfc191e 414 // Modified to be generated by Interrupt 2
afmiee 12:f361ccfc191e 415 void stop_smpl()
afmiee 12:f361ccfc191e 416 {
afmiee 12:f361ccfc191e 417 stop = 1;
afmiee 12:f361ccfc191e 418 start = 0;
afmiee 12:f361ccfc191e 419 //dbg1 = 0;
afmiee 12:f361ccfc191e 420 //dbg2 = 1;
afmiee 12:f361ccfc191e 421 }
afmiee 12:f361ccfc191e 422
afmiee 12:f361ccfc191e 423 void parp( int times )
afmiee 12:f361ccfc191e 424 {
afmiee 12:f361ccfc191e 425 int i;
afmiee 12:f361ccfc191e 426 for( i = 0; i < times; i++) {
afmiee 12:f361ccfc191e 427 led1 = LED_ON;
afmiee 12:f361ccfc191e 428 wait( 0.05);
afmiee 12:f361ccfc191e 429 led1 = LED_OFF;
afmiee 12:f361ccfc191e 430 wait( 0.05);
afmiee 12:f361ccfc191e 431 }
afmiee 12:f361ccfc191e 432 led2 = 1;
afmiee 12:f361ccfc191e 433 }
afmiee 12:f361ccfc191e 434
afmiee 12:f361ccfc191e 435 // Flipped every second
afmiee 12:f361ccfc191e 436 void flip()
afmiee 12:f361ccfc191e 437 {
afmiee 12:f361ccfc191e 438 led2 = LED_ON;
afmiee 12:f361ccfc191e 439 wait(0.01);
afmiee 12:f361ccfc191e 440 led2 = LED_OFF;
afmiee 12:f361ccfc191e 441 if ( start && !stop ) {
afmiee 12:f361ccfc191e 442 led1 = LED_ON;
afmiee 12:f361ccfc191e 443 wait(0.01);
afmiee 12:f361ccfc191e 444 led1 = LED_OFF;
afmiee 12:f361ccfc191e 445 }
afmiee 12:f361ccfc191e 446 }
afmiee 12:f361ccfc191e 447
afmiee 12:f361ccfc191e 448 void print_config_int_registers( void )
afmiee 12:f361ccfc191e 449 {
afmiee 12:f361ccfc191e 450 #ifdef DEBUG
afmiee 12:f361ccfc191e 451 debug.printf( "\n\r");
afmiee 12:f361ccfc191e 452 debug.printf( "INT1_CTRL (0Ch) %02x\n\r", I2CreadByte(0xD6, 0x0C));
afmiee 12:f361ccfc191e 453 debug.printf( "INT2_CTRL (0Dh) %02x\n\r", I2CreadByte(0xD6, 0x0D));
afmiee 12:f361ccfc191e 454 debug.printf( "CTRL_REG8 (22h) %02x\n\r", I2CreadByte(0xD6, 0x22));
afmiee 12:f361ccfc191e 455 debug.printf( "STATUS_REG (27h) %02x\n\r", I2CreadByte(0xD6, 0x27));
afmiee 12:f361ccfc191e 456 debug.printf( "\n\r");
afmiee 12:f361ccfc191e 457 debug.printf( "INT_GEN_CFG_XL (06h) %02x\n\r", I2CreadByte(0xD6, 0x06));
afmiee 12:f361ccfc191e 458 debug.printf( "INT_GEN_SRC_XL (26h) %02x\n\r", I2CreadByte(0xD6, 0x26));
afmiee 12:f361ccfc191e 459 debug.printf( "INT_GEN_THS_X_XL (07h) %02x\n\r", I2CreadByte(0xD6, 0x07));
afmiee 12:f361ccfc191e 460 debug.printf( "INT_GEN_THS_Y_XL (08h) %02x\n\r", I2CreadByte(0xD6, 0x08));
afmiee 12:f361ccfc191e 461 debug.printf( "INT_GEN_THS_Z_XL (09h) %02x\n\r", I2CreadByte(0xD6, 0x09));
afmiee 12:f361ccfc191e 462 debug.printf( "INT_GEN_DUR_XL (0ah) %02x\n\r", I2CreadByte(0xD6, 0x0a));
afmiee 12:f361ccfc191e 463 debug.printf( "\n\r");
afmiee 12:f361ccfc191e 464 debug.printf( "INT_GEN_CFG_G (30h) %02x\n\r", I2CreadByte(0xD6, 0x30));
afmiee 12:f361ccfc191e 465 debug.printf( "INT_GEN_SRC_G (14h) %02x\n\r", I2CreadByte(0xD6, 0x14));
afmiee 12:f361ccfc191e 466 debug.printf( "INT_GEN_THS_XH_G (31h) %02x\n\r", I2CreadByte(0xD6, 0x31));
afmiee 12:f361ccfc191e 467 debug.printf( "INT_GEN_THS_XL_G (32h) %02x\n\r", I2CreadByte(0xD6, 0x32));
afmiee 12:f361ccfc191e 468 debug.printf( "INT_GEN_THS_YH_G (33h) %02x\n\r", I2CreadByte(0xD6, 0x33));
afmiee 12:f361ccfc191e 469 debug.printf( "INT_GEN_THS_YL_G (34h) %02x\n\r", I2CreadByte(0xD6, 0x34));
afmiee 12:f361ccfc191e 470 debug.printf( "INT_GEN_THS_ZH_G (35h) %02x\n\r", I2CreadByte(0xD6, 0x35));
afmiee 12:f361ccfc191e 471 debug.printf( "INT_GEN_THS_ZL_G (36h) %02x\n\r", I2CreadByte(0xD6, 0x36));
afmiee 12:f361ccfc191e 472 debug.printf( "INT_GEN_DUR_G (37h) %02x\n\r", I2CreadByte(0xD6, 0x37));
afmiee 12:f361ccfc191e 473 #endif
afmiee 12:f361ccfc191e 474 }
afmiee 12:f361ccfc191e 475
afmiee 12:f361ccfc191e 476 void open_temp_files( void )
afmiee 12:f361ccfc191e 477 {
afmiee 12:f361ccfc191e 478 debug.printf("\n\r");
afmiee 12:f361ccfc191e 479 fpA = fopen("/sd/ACC.csv", "w");
afmiee 12:f361ccfc191e 480 // Verify that file can be created
afmiee 12:f361ccfc191e 481 if ( fpA == NULL ) {
afmiee 12:f361ccfc191e 482 debug.printf("Cannot create file ACC.csv\n\r");
afmiee 12:f361ccfc191e 483 wait(0.5);
afmiee 12:f361ccfc191e 484 while(1) {
afmiee 12:f361ccfc191e 485 led1 = !led1;
afmiee 12:f361ccfc191e 486 wait(0.5);
afmiee 12:f361ccfc191e 487 }
afmiee 12:f361ccfc191e 488 } else
afmiee 12:f361ccfc191e 489 debug.printf("File ACC.csv created successfully\n\r");
afmiee 12:f361ccfc191e 490
afmiee 12:f361ccfc191e 491 fpG = fopen("/sd/GYR.csv", "w");
afmiee 12:f361ccfc191e 492 // Verify that file can be created
afmiee 12:f361ccfc191e 493 if ( fpG == NULL ) {
afmiee 12:f361ccfc191e 494 debug.printf("Cannot create file GYR.csv\n\r");
afmiee 12:f361ccfc191e 495 wait(0.5);
afmiee 12:f361ccfc191e 496 while(1) {
afmiee 12:f361ccfc191e 497 led1 = !led1;
afmiee 12:f361ccfc191e 498 wait(0.5);
afmiee 12:f361ccfc191e 499 }
afmiee 12:f361ccfc191e 500 } else
afmiee 12:f361ccfc191e 501 debug.printf("File GYR.csv created successfully\n\r");
afmiee 12:f361ccfc191e 502
afmiee 12:f361ccfc191e 503 fpM = fopen("/sd/MAG.csv", "w");
afmiee 12:f361ccfc191e 504 // Verify that file can be created
afmiee 12:f361ccfc191e 505 if ( fpM == NULL ) {
afmiee 12:f361ccfc191e 506 debug.printf("Cannot create file MAG.csv\n\r");
afmiee 12:f361ccfc191e 507 wait(0.5);
afmiee 12:f361ccfc191e 508 while(1) {
afmiee 12:f361ccfc191e 509 led1 = !led1;
afmiee 12:f361ccfc191e 510 wait(0.5);
afmiee 12:f361ccfc191e 511 }
afmiee 12:f361ccfc191e 512 } else
afmiee 12:f361ccfc191e 513 debug.printf("File MAG.csv created successfully\n\r");
afmiee 12:f361ccfc191e 514 }
afmiee 12:f361ccfc191e 515
afmiee 12:f361ccfc191e 516 void rename_files( void )
afmiee 12:f361ccfc191e 517 {
afmiee 12:f361ccfc191e 518 sprintf(filename, "/sd/latch9DOFA_%08x.csv",seconds);
afmiee 12:f361ccfc191e 519 debug.printf("\n\r");
afmiee 12:f361ccfc191e 520 if((file_copy("/sd/ACC.csv",filename )) == 0) {
afmiee 12:f361ccfc191e 521 debug.printf("File ACC.csv copied successfully to %s\n\r", filename);
afmiee 12:f361ccfc191e 522 } else {
afmiee 12:f361ccfc191e 523 debug.printf("Error: unable to copy the file ACC.csv");
afmiee 12:f361ccfc191e 524 }
afmiee 12:f361ccfc191e 525 sprintf(filename, "/sd/latch9DOFM_%08x.csv",seconds);
afmiee 12:f361ccfc191e 526 if((file_copy("/sd/MAG.csv",filename )) == 0) {
afmiee 12:f361ccfc191e 527 debug.printf("File MAG.csv copied successfully to %s\n\r", filename);
afmiee 12:f361ccfc191e 528 } else {
afmiee 12:f361ccfc191e 529 debug.printf("Error: unable to copy the file MAG.csv");
afmiee 12:f361ccfc191e 530 }
afmiee 12:f361ccfc191e 531 sprintf(filename, "/sd/latch9DOFG_%08x.csv",seconds);
afmiee 12:f361ccfc191e 532 if((file_copy("/sd/GYR.csv",filename )) == 0) {
afmiee 12:f361ccfc191e 533 debug.printf("File GYR.csv copied successfully to %s\n\r", filename);
afmiee 12:f361ccfc191e 534 } else {
afmiee 12:f361ccfc191e 535 debug.printf("Error: unable to copy the file GYR.csv");
afmiee 12:f361ccfc191e 536 }
afmiee 12:f361ccfc191e 537 }
afmiee 12:f361ccfc191e 538
afmiee 12:f361ccfc191e 539 void DumpAccelGyroRegs( void )
afmiee 12:f361ccfc191e 540 {
afmiee 12:f361ccfc191e 541 char dest[ 0x34 ];
afmiee 12:f361ccfc191e 542 int i;
afmiee 12:f361ccfc191e 543
afmiee 12:f361ccfc191e 544 debug.printf("\n\r");
afmiee 12:f361ccfc191e 545 I2CreadBytes( 0xD6, 0x04, (uint8_t *)dest, 0x34 );
afmiee 12:f361ccfc191e 546
afmiee 12:f361ccfc191e 547 for( i = 0; i < 0x34; i++ ) {
afmiee 12:f361ccfc191e 548 //I2CreadByte(0xD6, i + 0x04);
afmiee 12:f361ccfc191e 549 switch( i + 0x04 ) {
afmiee 12:f361ccfc191e 550 case 0x04:
afmiee 12:f361ccfc191e 551 debug.printf("ACT_THS 0x04 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 552 break;
afmiee 12:f361ccfc191e 553 case 0x05:
afmiee 12:f361ccfc191e 554 debug.printf("ACT_DUR 0x05 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 555 break;
afmiee 12:f361ccfc191e 556 case 0x06:
afmiee 12:f361ccfc191e 557 debug.printf("INT_GEN_CFG_XL 0x06 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 558 break;
afmiee 12:f361ccfc191e 559 case 0x07:
afmiee 12:f361ccfc191e 560 debug.printf("INT_GEN_THS_X_XL 0x07 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 561 break;
afmiee 12:f361ccfc191e 562 case 0x08:
afmiee 12:f361ccfc191e 563 debug.printf("INT_GEN_THS_Y_XL 0x08 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 564 break;
afmiee 12:f361ccfc191e 565 case 0x09:
afmiee 12:f361ccfc191e 566 debug.printf("INT_GEN_THS_Z_XL 0x09 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 567 break;
afmiee 12:f361ccfc191e 568 case 0x0A:
afmiee 12:f361ccfc191e 569 debug.printf("INT_GEN_DUR_XL 0x0A %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 570 break;
afmiee 12:f361ccfc191e 571 case 0x0B:
afmiee 12:f361ccfc191e 572 debug.printf("REFERENCE_G 0x0B %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 573 break;
afmiee 12:f361ccfc191e 574 case 0x0C:
afmiee 12:f361ccfc191e 575 debug.printf("INT1_CTRL 0x0C %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 576 break;
afmiee 12:f361ccfc191e 577 case 0x0D:
afmiee 12:f361ccfc191e 578 debug.printf("INT2_CTRL 0x0D %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 579 break;
afmiee 12:f361ccfc191e 580 case 0x0F:
afmiee 12:f361ccfc191e 581 debug.printf("WHO_AM_I_XG 0x0F %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 582 break;
afmiee 12:f361ccfc191e 583 case 0x10:
afmiee 12:f361ccfc191e 584 debug.printf("CTRL_REG1_G 0x10 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 585 break;
afmiee 12:f361ccfc191e 586 case 0x11:
afmiee 12:f361ccfc191e 587 debug.printf("CTRL_REG2_G 0x11 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 588 break;
afmiee 12:f361ccfc191e 589 case 0x12:
afmiee 12:f361ccfc191e 590 debug.printf("CTRL_REG3_G 0x12 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 591 break;
afmiee 12:f361ccfc191e 592 case 0x13:
afmiee 12:f361ccfc191e 593 debug.printf("ORIENT_CFG_G 0x13 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 594 break;
afmiee 12:f361ccfc191e 595 case 0x14:
afmiee 12:f361ccfc191e 596 debug.printf("INT_GEN_SRC_G 0x14 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 597 break;
afmiee 12:f361ccfc191e 598 case 0x15:
afmiee 12:f361ccfc191e 599 debug.printf("OUT_TEMP_L 0x15 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 600 break;
afmiee 12:f361ccfc191e 601 case 0x16:
afmiee 12:f361ccfc191e 602 debug.printf("OUT_TEMP_H 0x16 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 603 break;
afmiee 12:f361ccfc191e 604 case 0x17:
afmiee 12:f361ccfc191e 605 debug.printf("STATUS_REG_0 0x17 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 606 break;
afmiee 12:f361ccfc191e 607 case 0x18:
afmiee 12:f361ccfc191e 608 debug.printf("OUT_X_L_G 0x18 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 609 break;
afmiee 12:f361ccfc191e 610 case 0x19:
afmiee 12:f361ccfc191e 611 debug.printf("OUT_X_H_G 0x19 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 612 break;
afmiee 12:f361ccfc191e 613 case 0x1A:
afmiee 12:f361ccfc191e 614 debug.printf("OUT_Y_L_G 0x1A %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 615 break;
afmiee 12:f361ccfc191e 616 case 0x1B:
afmiee 12:f361ccfc191e 617 debug.printf("OUT_Y_H_G 0x1B %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 618 break;
afmiee 12:f361ccfc191e 619 case 0x1C:
afmiee 12:f361ccfc191e 620 debug.printf("OUT_Z_L_G 0x1C %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 621 break;
afmiee 12:f361ccfc191e 622 case 0x1D:
afmiee 12:f361ccfc191e 623 debug.printf("OUT_Z_H_G 0x1D %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 624 break;
afmiee 12:f361ccfc191e 625 case 0x1E:
afmiee 12:f361ccfc191e 626 debug.printf("CTRL_REG4 0x1E %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 627 break;
afmiee 12:f361ccfc191e 628 case 0x1F:
afmiee 12:f361ccfc191e 629 debug.printf("CTRL_REG5_XL 0x1F %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 630 break;
afmiee 12:f361ccfc191e 631 case 0x20:
afmiee 12:f361ccfc191e 632 debug.printf("CTRL_REG6_XL 0x20 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 633 break;
afmiee 12:f361ccfc191e 634 case 0x21:
afmiee 12:f361ccfc191e 635 debug.printf("CTRL_REG7_XL 0x21 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 636 break;
afmiee 12:f361ccfc191e 637 case 0x22:
afmiee 12:f361ccfc191e 638 debug.printf("CTRL_REG8 0x22 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 639 break;
afmiee 12:f361ccfc191e 640 case 0x23:
afmiee 12:f361ccfc191e 641 debug.printf("CTRL_REG9 0x23 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 642 break;
afmiee 12:f361ccfc191e 643 case 0x24:
afmiee 12:f361ccfc191e 644 debug.printf("CTRL_REG10 0x24 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 645 break;
afmiee 12:f361ccfc191e 646 case 0x26:
afmiee 12:f361ccfc191e 647 debug.printf("INT_GEN_SRC_XL 0x26 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 648 break;
afmiee 12:f361ccfc191e 649 case 0x27:
afmiee 12:f361ccfc191e 650 debug.printf("STATUS_REG_1 0x27 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 651 break;
afmiee 12:f361ccfc191e 652 case 0x28:
afmiee 12:f361ccfc191e 653 debug.printf("OUT_X_L_XL 0x28 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 654 break;
afmiee 12:f361ccfc191e 655 case 0x29:
afmiee 12:f361ccfc191e 656 debug.printf("OUT_X_H_XL 0x29 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 657 break;
afmiee 12:f361ccfc191e 658 case 0x2A:
afmiee 12:f361ccfc191e 659 debug.printf("OUT_Y_L_XL 0x2A %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 660 break;
afmiee 12:f361ccfc191e 661 case 0x2B:
afmiee 12:f361ccfc191e 662 debug.printf("OUT_Y_H_XL 0x2B %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 663 break;
afmiee 12:f361ccfc191e 664 case 0x2C:
afmiee 12:f361ccfc191e 665 debug.printf("OUT_Z_L_XL 0x2C %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 666 break;
afmiee 12:f361ccfc191e 667 case 0x2D:
afmiee 12:f361ccfc191e 668 debug.printf("OUT_Z_H_XL 0x2D %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 669 break;
afmiee 12:f361ccfc191e 670 case 0x2E:
afmiee 12:f361ccfc191e 671 debug.printf("FIFO_CTRL 0x2E %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 672 break;
afmiee 12:f361ccfc191e 673 case 0x2F:
afmiee 12:f361ccfc191e 674 debug.printf("FIFO_SRC 0x2F %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 675 break;
afmiee 12:f361ccfc191e 676 case 0x30:
afmiee 12:f361ccfc191e 677 debug.printf("INT_GEN_CFG_G 0x30 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 678 break;
afmiee 12:f361ccfc191e 679 case 0x31:
afmiee 12:f361ccfc191e 680 debug.printf("INT_GEN_THS_XH_G 0x31 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 681 break;
afmiee 12:f361ccfc191e 682 case 0x32:
afmiee 12:f361ccfc191e 683 debug.printf("INT_GEN_THS_XL_G 0x32 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 684 break;
afmiee 12:f361ccfc191e 685 case 0x33:
afmiee 12:f361ccfc191e 686 debug.printf("INT_GEN_THS_YH_G 0x33 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 687 break;
afmiee 12:f361ccfc191e 688 case 0x34:
afmiee 12:f361ccfc191e 689 debug.printf("INT_GEN_THS_YL_G 0x34 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 690 break;
afmiee 12:f361ccfc191e 691 case 0x35:
afmiee 12:f361ccfc191e 692 debug.printf("INT_GEN_THS_ZH_G 0x35 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 693 break;
afmiee 12:f361ccfc191e 694 case 0x36:
afmiee 12:f361ccfc191e 695 debug.printf("INT_GEN_THS_ZL_G 0x36 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 696 break;
afmiee 12:f361ccfc191e 697 case 0x37:
afmiee 12:f361ccfc191e 698 debug.printf("INT_GEN_DUR_G 0x37 %02x\n\r", dest[i]);
afmiee 12:f361ccfc191e 699 break;
afmiee 12:f361ccfc191e 700 default:
afmiee 12:f361ccfc191e 701 debug.printf("Register Not Valid 0x%02x\n\r");
afmiee 12:f361ccfc191e 702 break;
afmiee 12:f361ccfc191e 703 }
afmiee 12:f361ccfc191e 704 }
afmiee 12:f361ccfc191e 705 }
afmiee 12:f361ccfc191e 706
afmiee 12:f361ccfc191e 707
afmiee 12:f361ccfc191e 708
afmiee 12:f361ccfc191e 709 #define DEPTH 64
afmiee 12:f361ccfc191e 710 #define DEPP2 6
afmiee 12:f361ccfc191e 711
afmiee 12:f361ccfc191e 712
afmiee 12:f361ccfc191e 713 ulint mag_vec( int ax, int ay, int az )
afmiee 12:f361ccfc191e 714 {
afmiee 12:f361ccfc191e 715 static int x[DEPTH];
afmiee 12:f361ccfc191e 716 static int y[DEPTH];
afmiee 12:f361ccfc191e 717 static int z[DEPTH];
afmiee 12:f361ccfc191e 718
afmiee 12:f361ccfc191e 719 int i;
afmiee 12:f361ccfc191e 720 int sx,sy,sz;
afmiee 12:f361ccfc191e 721
afmiee 12:f361ccfc191e 722 x[0] = ax;
afmiee 12:f361ccfc191e 723 y[0] = ay;
afmiee 12:f361ccfc191e 724 z[0] = az;
afmiee 12:f361ccfc191e 725 sx = 0;
afmiee 12:f361ccfc191e 726 sy = 0;
afmiee 12:f361ccfc191e 727 sz = 0;
afmiee 12:f361ccfc191e 728
afmiee 12:f361ccfc191e 729 for( i = 0; i < DEPTH; i++ ) {
afmiee 12:f361ccfc191e 730 sx+= x[i];
afmiee 12:f361ccfc191e 731 sy+= y[i];
afmiee 12:f361ccfc191e 732 sz+= z[i];
afmiee 12:f361ccfc191e 733 }
afmiee 12:f361ccfc191e 734 sx >>= DEPP2;
afmiee 12:f361ccfc191e 735 sy >>= DEPP2;
afmiee 12:f361ccfc191e 736 sz >>= DEPP2;
afmiee 12:f361ccfc191e 737
afmiee 12:f361ccfc191e 738 for( i = 0; i < DEPTH-1; i++ ) {
afmiee 12:f361ccfc191e 739 x[i+1] = x[i];
afmiee 12:f361ccfc191e 740 y[i+1] = y[i];
afmiee 12:f361ccfc191e 741 z[i+1] = z[i];
afmiee 12:f361ccfc191e 742 }
afmiee 12:f361ccfc191e 743 return( (ulint)(sx*sx + sy*sy + sz*sz) );
afmiee 12:f361ccfc191e 744 }
afmiee 12:f361ccfc191e 745
afmiee 12:f361ccfc191e 746
afmiee 12:f361ccfc191e 747 ulint maxvec( ulint vec, int reset )
afmiee 12:f361ccfc191e 748 {
afmiee 12:f361ccfc191e 749 ulint static max;
afmiee 12:f361ccfc191e 750
afmiee 12:f361ccfc191e 751 if( reset == 0 )
afmiee 12:f361ccfc191e 752 max = 0;
afmiee 12:f361ccfc191e 753
afmiee 12:f361ccfc191e 754 if ( vec > max ) {
afmiee 12:f361ccfc191e 755 max = vec;
afmiee 12:f361ccfc191e 756 }
afmiee 12:f361ccfc191e 757 return( max );
afmiee 12:f361ccfc191e 758 }
afmiee 12:f361ccfc191e 759
afmiee 12:f361ccfc191e 760 ulint minvec( ulint vec, int reset )
afmiee 12:f361ccfc191e 761 {
afmiee 12:f361ccfc191e 762 ulint static min;
afmiee 12:f361ccfc191e 763 if ( reset == 0)
afmiee 12:f361ccfc191e 764 min = 0xFFFFFFFF;
afmiee 12:f361ccfc191e 765
afmiee 12:f361ccfc191e 766 if ( vec < min ) {
afmiee 12:f361ccfc191e 767 min = vec;
afmiee 12:f361ccfc191e 768 }
afmiee 12:f361ccfc191e 769 return( min );
afmiee 12:f361ccfc191e 770
afmiee 12:f361ccfc191e 771 }
afmiee 12:f361ccfc191e 772
afmiee 12:f361ccfc191e 773