Antonio Mondragon / Mbed 2 deprecated Latch9DOF_LSM9DS1

Minor fixes

Dependencies:   LSM9DS1_Library SDFileSystem mbed nrf51_rtc

Fork of LSM303DLHTest by Toshihisa T

main.cpp@11:a246c67d44b0, 2016-07-27 (annotated)

Committer:
afmiee
Date:
Wed Jul 27 21:55:46 2016 +0000
Revision:
11:a246c67d44b0
Parent:
10:e7ab0493ed9f
Child:
12:f361ccfc191e
This version can do Buttons or motion depending on the MOTION definition. It also has FIFO enable to try to capture data when the door is being open. The best accelerometer threshold was 0x0C.

Who changed what in which revision?

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