accelerometer.cpp

00001 #include "Custom_LSM303/accelerometer.h"
00002 
00003 #define LSM303_REG_ACC_STATUS_REG_AUX_A 0x07
00004 //#define LSM303_REG_ACC_OUT_TEMP_L_A 0x0C
00005 //#define LSM303_REG_ACC_OUT_TEMP_H_A 0x0D
00006 #define LSM303_REG_ACC_WHO_AM_I_A 0x0F
00007 //#define LSM303_REG_ACC_TEMP_CFG_REG_A 0x1F
00008 #define LSM303_REG_ACC_CTRL_REG1_A 0x20
00009 #define LSM303_REG_ACC_CTRL_REG2_A 0x21
00010 #define LSM303_REG_ACC_CTRL_REG3_A 0x22
00011 #define LSM303_REG_ACC_CTRL_REG4_A 0x23
00012 #define LSM303_REG_ACC_CTRL_REG5_A 0x24
00013 #define LSM303_REG_ACC_CTRL_REG6_A 0x25
00014 #define LSM303_REG_ACC_STATUS_REG_A 0x27
00015 #define LSM303_REG_ACC_OUT_X_L_A 0x28
00016 #define LSM303_REG_ACC_OUT_X_H_A 0x29
00017 #define LSM303_REG_ACC_OUT_Y_L_A 0x2A
00018 #define LSM303_REG_ACC_OUT_Y_H_A 0x2B
00019 #define LSM303_REG_ACC_OUT_Z_L_A 0x2C
00020 #define LSM303_REG_ACC_OUT_Z_H_A 0x2D
00021 #define LSM303_REG_ACC_INT1_CFG_A 0x30
00022 #define LSM303_REG_ACC_INT1_SRC_A 0x31
00023 #define LSM303_REG_ACC_INT1_THS_A 0x32
00024 #define LSM303_REG_ACC_INT1_DURATION_A 0x33
00025 
00026 #if defined(TARGET_LRAT)
00027     #define PIN_ACC 0x1E
00028     
00029     #define LSM303_ADR_ACC 0x3A
00030     #define LSM303_WHO_ACC 0x41
00031 
00032     #define CFG_ACC_ADR LSM303_REG_ACC_CTRL_REG1_A
00033     #define CFG_ACC_LEN 7
00034 
00035     #define LSM303_REG_ACC_INT1_THS_X1_A 0x32
00036     #define LSM303_REG_ACC_INT1_THS_Y1_A 0x33
00037     #define LSM303_REG_ACC_INT1_THS_Z1_A 0x34
00038     #define LSM303_REG_ACC_INT1_DURATION_A 0x35
00039     #define LSM303_REG_ACC_INT2_CFG_A 0x36
00040     #define LSM303_REG_ACC_INT2_SRC_A 0x37
00041     #define LSM303_REG_ACC_INT2_THS_A 0x38
00042     #define LSM303_REG_ACC_INT2_DURATION_A 0x39
00043     #define LSM303_REG_ACC_XL_REFERENCE 0x3A
00044     #define LSM303_REG_ACC_XH_REFERENCE 0x3B
00045     #define LSM303_REG_ACC_YL_REFERENCE 0x3C
00046     #define LSM303_REG_ACC_YH_REFERENCE 0x3D
00047     #define LSM303_REG_ACC_ZL_REFERENCE 0x3E
00048     #define LSM303_REG_ACC_ZH_REFERENCE 0x3F
00049 #else
00050     #define PIN_ACC 0x1E
00051 
00052     #define LSM303_ADR_ACC 0x32
00053     #define LSM303_WHO_ACC 0x33
00054     
00055     #define LSM303_REG_ACC_OUT_TEMP_L_A 0x0C
00056     #define LSM303_REG_ACC_OUT_TEMP_H_A 0x0D
00057     #define LSM303_REG_ACC_TEMP_CFG_REG_A 0x1F
00058     
00059     #define CFG_ACC_ADR LSM303_REG_ACC_TEMP_CFG_REG_A // Start Disco at TEMP CFG.
00060     #define CFG_ACC_LEN 7 
00061 #endif
00062 
00063 
00064 char sPass[26] = "[\u001b[32mPASS\u001b[0m]";
00065 char sFail[26] = "[\u001b[31mFAIL\u001b[0m]";
00066 
00067 /**
00068  */
00069 Accelerometer::Accelerometer()
00070 {
00071     accPin = InterruptIn(PIN_ACC);
00072 }
00073 
00074 /**
00075  */
00076 Accelerometer::~Accelerometer()
00077 {
00078 
00079 }
00080 
00081 /**
00082  */
00083 void Accelerometer::setters(I2C *_i2c, char *_buf)
00084 {
00085     i2c = _i2c;
00086     buf = _buf;
00087 }
00088 
00089 /**
00090  */
00091 void Accelerometer::dumpCfg()
00092 {
00093     char cmd[2];
00094     char start = CFG_ACC_ADR;
00095     
00096     for (int i = 0; i < CFG_ACC_LEN; i++)
00097     {
00098         cmd[0] = start + i;
00099         i2c->write(LSM303_ADR_ACC, cmd, 1);
00100         i2c->read(LSM303_ADR_ACC, &buf[i], 1);
00101     }
00102     
00103     printf("CFGACC: | ");
00104     for (int i = 0; i < CFG_ACC_LEN; i++)
00105     {
00106         printf("%02X ", buf[i]);
00107     }
00108     printf("|\r\n");
00109 }
00110 
00111 /**
00112  */
00113 void Accelerometer::onIRQ()
00114 {
00115     // ACCELEROMETER INTERRUPT FUNCTION
00116 }
00117 
00118 /**
00119  */
00120 void Accelerometer::init()
00121 {
00122     printf("In Accelerometer Init Sequence...\r\n");
00123     
00124     char *res;
00125     char ret;
00126     
00127     char cmd[2];
00128     cmd[0] = LSM303_REG_ACC_WHO_AM_I_A;
00129     
00130     i2c->write(LSM303_ADR_ACC, cmd, 1);
00131     i2c->read(LSM303_ADR_ACC, &ret, 1);
00132     
00133     res = (ret == LSM303_WHO_ACC ? sPass : sFail);
00134     
00135     printf("ACC WhoAmI: %02X %s\r\n", ret, res);
00136     
00137 #if defined(TARGET_LRAT)
00138     cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
00139     cmd[1] = 0xB7;  // High Resolution, ODR=100Hz, Enable XYZ Axes
00140     i2c->write(LSM303_ADR_ACC, cmd, 2);
00141     
00142     // HUOT ADDITION -- 08/30/2018 -- Accelerometer Intterupt Init Sequence
00143     #if defined(ACC_INTERRUPT_ON)
00144         // ACC INTERRUPT SETUP
00145         // Enable Interrupt Pin
00146         cmd[0] = LSM303_REG_ACC_CTRL_REG3_A;
00147         i2c->write(LSM303_ADR_ACC, cmd, 1);
00148         i2c->read(LSM303_ADR_ACC, &ret, 1);
00149         
00150         // Configure Control Register 3
00151         cmd[0] = LSM303_REG_ACC_CTRL_REG3_A;
00152         cmd[1] = 0x08;
00153         i2c->write(LSM303_ADR_ACC, cmd, 2);
00154     
00155         cmd[0] = LSM303_CTRL_REG7_A;
00156         i2c->write(LSM303_ADR_ACC, cmd, 1);
00157         i2c->read(LSM303_ADR_ACC, &ret, 1);
00158         
00159         // Latch Interrupt Enabling
00160         cmd[0] = LSM303_CTRL_REG7_A;
00161         cmd[1] = 0x04;
00162         i2c->write(LSM303_ADR_ACC, cmd, 2);
00163         
00164         // Set Threshold/Duration/Config
00165         // Set Threshold on X axis
00166         cmd[0] = LSM303_REG_ACC_INT1_THS_X1_A;
00167         cmd[1] = 0x7D;
00168         i2c->write(LSM303_ADR_ACC, cmd, 2);
00169         
00170         // Set Threshold on Y axis
00171         cmd[0] = LSM303_REG_ACC_INT1_THS_Y1_A;
00172         i2c->write(LSM303_ADR_ACC, cmd, 2);
00173         
00174         // Set Y Threshold on Z axis
00175         cmd[0] = LSM303_REG_ACC_INT1_THS_Z1_A;
00176         i2c->write(LSM303_ADR_ACC, cmd, 2);
00177         
00178         // Set Interrupt Duration
00179         cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
00180         cmd[1] = 0x00;
00181         i2c->write(LSM303_ADR_ACC, cmd, 2);
00182         
00183         // Set Interrupt Configuration
00184         cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
00185         cmd[1] = 0x02;  // Set Interrupt Generation on X-Axis High event
00186         i2c->write(LSM303_ADR_ACC, cmd, 2);
00187         accPin.rise(&onIRQ);
00188     #endif
00189 #else
00190     cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
00191     cmd[1] = 0x57;  // Enable XYZ Axes, ODR=100Hz
00192     i2c->write(LSM303_ADR_ACC, cmd, 2);
00193     
00194     // Enable High Resolution Mode
00195     cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
00196     i2c->write(LSM303_ADR_ACC, cmd, 1);
00197     i2c->read(LSM303_ADR_ACC, &ret, 1);
00198     
00199     cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
00200     cmd[1] = ret | 0x08;    // High Resolution
00201     i2c->write(LSM303_ADR_ACC, cmd, 2);
00202     
00203     #if defined(ENABLE_TMPSENSOR)
00204         // Enable Temp Sensor
00205         cmd[0] = LSM303_REG_ACC_TEMP_CFG_REG_A;
00206         cmd[1] = 0xC0;
00207         i2c->write(LSM303_ADR_ACC, cmd, 2);
00208         
00209         cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
00210         i2c->write(LSM303_ADR_ACC, cmd, 1);
00211         i2c->read(LSM303_ADR_ACC, &ret, 1);
00212         
00213         cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
00214         cmd[1] = ret | 0x80;
00215         i2c->write(LSM303_ADR_ACC, cmd, 2);
00216     #endif
00217 #endif
00218 }
00219 
00220 // DO WE EVEN NEED OR USE THIS METHOD?!?!?!?
00221 /*
00222 void read()
00223 {
00224     cmd[0] = LSM303_REG_ACC_STATUS_REG_A;
00225     i2c.write(LSM303_ADR_ACC, cmd, 1);
00226     i2c.read(LSM303_ADR_ACC, &rda, 1);
00227     if (rda)
00228     {
00229     cmd[0] = LSM303_REG_ACC_OUT_X_L_A;
00230     i2c.write(LSM303_ADR_ACC, cmd, 1);
00231     i2c.read(LSM303_ADR_ACC, &buf[0], 1);
00232     cmd[0] = LSM303_REG_ACC_OUT_X_H_A;
00233     i2c.write(LSM303_ADR_ACC, cmd, 1);
00234     i2c.read(LSM303_ADR_ACC, &buf[1], 1);
00235     cmd[0] = LSM303_REG_ACC_OUT_Y_L_A;
00236     i2c.write(LSM303_ADR_ACC, cmd, 1);
00237     i2c.read(LSM303_ADR_ACC, &buf[2], 1);
00238     cmd[0] = LSM303_REG_ACC_OUT_Y_H_A;
00239     i2c.write(LSM303_ADR_ACC, cmd, 1);
00240     i2c.read(LSM303_ADR_ACC, &buf[3], 1);
00241     cmd[0] = LSM303_REG_ACC_OUT_Z_L_A;
00242     i2c.write(LSM303_ADR_ACC, cmd, 1);
00243     i2c.read(LSM303_ADR_ACC, &buf[4], 1);
00244     cmd[0] = LSM303_REG_ACC_OUT_Z_H_A;
00245     i2c.write(LSM303_ADR_ACC, cmd, 1);
00246     i2c.read(LSM303_ADR_ACC, &buf[5], 1);
00247     myAccX = ((int16_t)(buf[0] | (buf[1] << 8)) >> accShift);
00248     myAccY = ((int16_t)(buf[2] | (buf[3] << 8)) >> accShift);
00249     myAccZ = ((int16_t)(buf[4] | (buf[5] << 8)) >> accShift);
00250     if (myAccX < accMinX)
00251         accMinX = myAccX;
00252     if (myAccY < accMinY)
00253         accMinY = myAccY;
00254     if (myAccZ < accMinZ)
00255         accMinZ = myAccZ;
00256     if (myAccX > accMaxX)
00257         accMaxX = myAccX;
00258     if (myAccY > accMaxY)
00259         accMaxY = myAccY;
00260     if (myAccZ > accMaxZ)
00261         accMaxZ = myAccZ;
00262     cmd[0] = LSM303_REG_ACC_INT1_SRC_A;
00263     i2c.write(LSM303_ADR_ACC, cmd, 1);
00264     i2c.read(LSM303_ADR_ACC, &ret, 1);
00265     if (ret & 0x40)
00266     {
00267         accSFire++;
00268         if (accEvent == 1)
00269         {
00270             accEvent = 0;
00271             accSLIRQ++;
00272             cmd[0] = LSM303_REG_ACC_INT1_THS_A;
00273             cmd[1] = 0x7D;
00274             i2c.write(LSM303_ADR_ACC, cmd, 2);
00275             cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
00276             cmd[1] = 0x00;
00277             i2c.write(LSM303_ADR_ACC, cmd, 2);
00278             cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
00279             cmd[1] = 0x2A;
00280             i2c.write(LSM303_ADR_ACC, cmd, 2);
00281         }
00282         else
00283         {
00284             accEvent = 1;
00285             accSHIRQ++;
00286             cmd[0] = LSM303_REG_ACC_INT1_THS_A;
00287             cmd[1] = 0x50;
00288             i2c.write(LSM303_ADR_ACC, cmd, 2);
00289             cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
00290             //cmd[1] = 0x7D;
00291             cmd[1] = 0x03;
00292             i2c.write(LSM303_ADR_ACC, cmd, 2);
00293             cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
00294             cmd[1] = 0x95;
00295             i2c.write(LSM303_ADR_ACC, cmd, 2);
00296         }
00297     }
00298     printf("A|%02X|%02X %02X %02X %02X %02X %02X|%*d,%*d,%*d|%*d,%*d,%*d|%*d,%*d,%*d|%02X|%02X/%02X %02X/%02X\r\n", rda, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], 6, myAccX, 6, myAccY, 6, myAccZ, 6, accMinX, 6, accMinY, 6, accMinZ, 6, accMaxX, 6, accMaxY, 6, accMaxZ, ret, accSHIRQ, accSLIRQ, accSFire, accHFire);
00299     }
00300 }
00301 */