Revision 0:91b1274ec397, committed 2014-03-18

Comitter:

liamg

Date:

Tue Mar 18 05:22:01 2014 +0000

Child:

1:3b2260aff305

Commit message:

Completed without multibyte read function

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MKI124V1.h	Tue Mar 18 05:22:01 2014 +0000
@@ -0,0 +1,77 @@
+// Defines for the MEMS sensors on the STEVAL-MKI124V1 board
+// Liam Goudge. March 2014
+
+// LPS331AP MEMS pressure and temperature sensor
+#define LPS331addr      0xBA // I2C address of the sensor for writes (line SAO is tied high on this board so bit 1=1. Note this may not work on other boards).
+
+#define pWHO_AM_I        0x0F // "ping" register. Device will respond even when in sleep mode
+#define pRES_CONF        0x10 // Reset value is 0x7A (512 pressure samples and 128 temperature samples)
+#define pCTRL_REG1       0x20
+#define pCTRL_REG2       0x21
+#define pCTRL_REG3       0x22
+#define pPRESS_OUT_XL    0x28
+#define pPRESS_OUT_L     0x29
+#define pPRESS_OUT_H     0x2A
+#define pTEMP_OUT_L      0x2B // Temperature data. 2's complement. Device will respond when in sleep mode
+#define pTEMP_OUT_H      0x2C
+
+// LSM303DLHC MEMS ascceleratometer, magnetometer and temperature sensor
+#define LSM303_m        0x3C // I2C base address of the magnetometer sensor
+#define LSM303_a        0x32 // I2C base address of the accelerometer sensor
+
+// Magnetometer registers
+#define mCRA_REG_M       0x00 // Magnetic sensor configuration register A
+#define mCRB_REG_M       0x01 // Magnetic sensor configuration register B
+#define mMR_REG_M        0x02 // Magnetometer mode register
+#define mOUT_X_H_M       0x03 // X-axis magnetic field data High byte
+#define mOUT_X_L_M       0x04 // X-axis magnetic field data Low byte
+#define mOUT_Z_H_M       0x05 // Z-axis magnetic field data High byte
+#define mOUT_Z_L_M       0x06 // Z-axis magnetic field data Low byte
+#define mOUT_Y_H_M       0x07 // Y-axis magnetic field data High byte
+#define mOUT_Y_L_M       0x08 // Y-axis magnetic field data Low byte
+#define mSR_REG_M        0x09 // Magnetometer status register
+#define mIRA_REG_M       0x0A // ID register A. Should read 0x48
+#define mCTRL_REG1_A     0x20
+#define mTEMP_OUT_H_M    0x31 // Magnetometer temperature register High byte
+#define mTEMP_OUT_L_M    0x32 // Magnetometer temperature register Low byte
+
+// Accelerometer registers
+#define aCTRL_REG1_A     0x20 // Accelerometer output data rate ODR, low power modes etc
+#define aCTRL_REG4_A     0x23 // Accelerometer full scale selection etc
+#define aOUT_X_L_A       0x28 // Accelerometer X axis low byte
+#define aOUT_X_H_A       0x29 // Accelerometer X axis high byte
+#define aOUT_Y_L_A       0x2A // Accelerometer Y axis low byte
+#define aOUT_Y_H_A       0x2B // Accelerometer Y axis high byte
+#define aOUT_Z_L_A       0x2C // Accelerometer Z axis low byte
+#define aOUT_Z_H_A       0x2D // Accelerometer Z axis high byte
+
+// L3GD20 MEMS gyro. 3 axis angular rate sensor
+#define L3GD20_ADDR     0xD6 // I2C base address of the gyro
+
+#define gWHO_AM_I        0x0F // Ping register. Response is 0xD4. Responds even when device is off
+#define gCTRL_REG1       0x20 // Sets Output Data Rate, Bandwidth and Power mode
+#define gCTRL_REG4       0x23 // 
+#define gOUT_TEMP        0x26 // Temperature data. 8 bit resolution
+#define gOUT_X_L         0x28 // X-axis angular rate low byte expressed as 2's complement
+#define gOUT_X_H         0x29 // X-axis angular rate high byte
+#define gOUT_Y_L         0x2A
+#define gOUT_Y_H         0x2B
+#define gOUT_Z_L         0x2C
+#define gOUT_Z_H         0x2D
+
+
+// Structs
+typedef struct{   
+    float   pitch;
+    float   roll;
+    float   Xrotation;
+    float   Yrotation;
+    float   Zrotation;
+    float   heading;
+    float   tempC;
+    float   pressuremB;
+    } SensorState_t;
+    
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 18 05:22:01 2014 +0000
@@ -0,0 +1,218 @@
+// MBED reference code for the ST Micro STEVAL-MKI124V1 header board
+// This board has: LPS331 pressure/temperature sensor, L3GD20 gyroscope and LSM303DLHC magnetometer/accelerometer
+// Code accesses each of the 3 MEMS sensors and calculates pressure, temp, heading, tilt, roll and angular velocity
+// Code is not optimized for efficienecy but instead for clarity of how you use the sensors
+// ST application note AN3192 was key in developing the tilt-corrected compass
+// Developed on an LPC1768
+// By Liam Goudge. March 2014
+
+#include "mbed.h"
+#include "MKI124V1.h"
+#include "math.h"
+
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);    // tx, rx for USB debug printf to terminal console
+I2C i2c(p28, p27);          // LPC1768 I2C pin allocation
+
+// Globals
+int16_t const   Offset_mX=-40.0;
+int16_t const   Offset_mY=-115.0;
+float const     RadtoDeg=(180.0/3.141592654);
+
+
+char readByte(char address, char reg)
+// Reads one byte from an I2C address
+// Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
+// the data registers of all sensors they are in the same XL,XH,YL,YH,ZL,ZH order apart from the magnetometer which is XH,XL,ZH,ZL,YH,YL...
+{
+    char result;
+    
+    i2c.start();
+    i2c.write(address);         // Slave address with direction=write
+    i2c.write(reg);             // Subaddress (register)
+    
+    i2c.start();                // Break transmission to change bus direction
+    i2c.write(address + 1);     // Slave address with direction=read [bit0=1]
+    
+    result = i2c.read(0);
+    i2c.stop();
+    return (result);
+    }
+    
+void writeByte(char address, char reg,char value)
+// Sends 1 byte to an I2C address
+{    
+    i2c.start();
+    i2c.write(address);         // Slave address
+    i2c.write(reg);             // Subaddress (register)
+    i2c.write(value);
+    i2c.stop();
+    
+    }
+    
+void initSensors (void)
+// Switch on and set up the 3 on-board sensors
+{
+    pc.printf("--------------------------------------\n");
+    pc.printf("\nSTM MEMS eval board sensor init \n");
+    
+    // LSM303DLHC Magnetic sensor
+    pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
+    writeByte(LSM303_m,mCRA_REG_M,0x94);     //switch on temperature sensor and set Output Data Rate to 30Hz
+    writeByte(LSM303_m,mCRB_REG_M,0x20);     // Set the gain for +/- 1.3 Gauss full scale range
+    writeByte(LSM303_m,mMR_REG_M,0x0);       // Continuous convertion mode
+    
+    // LSM303DLHC Accelerometer
+    writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
+    writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
+    
+    // LPS331 Pressure sensor
+    pc.printf("LPS331 ping (should reply 0xBB): %x \n",readByte(LPS331addr,pWHO_AM_I));
+    writeByte(LPS331addr,pCTRL_REG1,0x90);  // Switch on pressure sensor and select 1Hz ODR. If you select one-shot then sensor powers itself down after every reading...
+    writeByte(LPS331addr,pRES_CONF,0x70);   // Temp and pressure noise reduction. Sets # of internal measurements that are averaged to 1 reading. Default is 0x7A (temp rez=128, press=512)
+
+    // L3GD20 gyro
+    printf("Ping L3GD20 (should reply 0xD4): %x \n",readByte(L3GD20_ADDR,gWHO_AM_I));
+    writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F); // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device
+    writeByte(L3GD20_ADDR,gCTRL_REG4,0x10); // Full scale selected at 500dps (degrees per second)
+    printf("L3GD20 gyro Temp: %x degrees C \n",readByte(L3GD20_ADDR,gOUT_TEMP));
+    
+    pc.printf("--------------------------------------\n \n");
+    wait(1); // Wait for settings to stabilize
+    }    
+    
+void LPS331(SensorState_t state)
+// Read the pressure sensor
+{
+    uint8_t             tempL,tempH,pressXL,pressL,pressH;
+    int16_t             temp;
+    int32_t             press24;
+    float               pressure;
+    
+    // Measure temperature
+    tempL=readByte(LPS331addr,pTEMP_OUT_L);
+    tempH=readByte(LPS331addr,pTEMP_OUT_H);
+    temp=(tempH << 8) | tempL; // 16-bit 2's complement data
+    
+    state.tempC=((float) temp / 480.0) + 42.5; // per equation on page 29 of the spec
+
+    pc.printf("Pressure sensor temperature %.1f degreesC \n",state.tempC);
+    
+    // Pressure test
+    pressXL=readByte(LPS331addr,pPRESS_OUT_XL);
+    pressL=readByte(LPS331addr,pPRESS_OUT_L);
+    pressH=readByte(LPS331addr,pPRESS_OUT_H);
+    
+    press24=(pressH << 16) | (pressL << 8) | pressXL ; // 24-bit 2's complement data
+    pressure = (float)press24/4096.0; // Sensitivity is 4096 LSB per milibar
+    
+    pc.printf("Pressure %.1f mbars or %.1f inches Hg\n", pressure, (pressure*0.0295)+0.029);
+
+    }
+
+void LSM303 (SensorState_t * state)
+// Magnetometer and accelerometer
+{
+    char        xL, xH, yL, yH, zL, zH;
+    int16_t     mX, mY, mZ,aX,aY,aZ;
+    float       pitch,roll,faX,faY;
+
+    xL=readByte(LSM303_m,mOUT_X_L_M);
+    xH=readByte(LSM303_m,mOUT_X_H_M);
+    yL=readByte(LSM303_m,mOUT_Y_L_M);
+    yH=readByte(LSM303_m,mOUT_Y_H_M);
+    zL=readByte(LSM303_m,mOUT_Z_L_M);
+    zH=readByte(LSM303_m,mOUT_Z_H_M);
+    
+    mX=(xH<<8) | (xL); // 16-bit 2's complement data
+    mY=(yH<<8) | (yL);
+    mZ=(zH<<8) | (zL);
+    
+    //pc.printf("mX=%hd   %X          mY=%hd  %X      mZ=%hd  %X      \n",mX,mX,mY,mY,mZ,mZ);
+    
+    mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
+    mY=mY-Offset_mY; 
+    
+    xL=readByte(LSM303_a,aOUT_X_L_A);
+    xH=readByte(LSM303_a,aOUT_X_H_A);
+    yL=readByte(LSM303_a,aOUT_Y_L_A);
+    yH=readByte(LSM303_a,aOUT_Y_H_A);
+    zL=readByte(LSM303_a,aOUT_Z_L_A);
+    zH=readByte(LSM303_a,aOUT_Z_H_A);
+
+    aX=(signed short) ( (xH<<8) | (xL) ) >> 4; // 12-bit data from ADC. Cast ensures that the 2's complement sign is not lost in the right shift.
+    aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
+    aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
+    
+    //pc.printf("aX=%hd   %X          aY=%hd  %X      aZ=%hd  %X      \n",aX,aX,aY,aY,aZ,aZ);
+    
+    faX=((float) aX) /2000.0; // Accelerometer scale I chose is 1mg per LSB with range +/-2g. So to normalize for full scale need to divide by 2000.
+    faY=((float) aY) /2000.0; // If you don't do this the pitch and roll calcs will not work (inverse cosine of a value greater than 1)
+    //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
+        
+    // Trigonometry derived from STM app note AN3192 and from WikiRobots
+   pitch = asin((float) -faX*2); // Dividing faX and faY by 1000 rather than 2000 seems to give better tilt immunity. Do it here rather than above to preserve true mg units of faX etc
+   roll = asin(faY*2/cos(pitch));
+   
+  float xh = mX * cos(pitch) + mZ * sin(pitch);
+  float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
+  float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
+ 
+  float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
+  if (yh < 0)
+    heading=360+heading;
+    
+    state->heading=heading;
+    state->pitch=pitch;
+    state->roll=roll;
+    
+    pc.printf("Orientation (deg):       Pitch: %5.1f         Roll: %5.1f          Heading: %5.1f     \n",pitch*RadtoDeg,roll*RadtoDeg,heading);
+    pc.printf("Acceleration (mg):       Forward: %5hd       Left: %5hd          Up: %5hd \n",aX,aY,aZ);    
+    
+}
+
+void    L3GD20(void) // Gyro
+{
+    char        xL, xH, yL, yH, zL, zH;
+    int16_t     gX, gY, gZ;
+    float       rorX,rorY,rorZ;
+    
+    xL=readByte(L3GD20_ADDR,gOUT_X_L);
+    xH=readByte(L3GD20_ADDR,gOUT_X_H);
+    yL=readByte(L3GD20_ADDR,gOUT_Y_L);
+    yH=readByte(L3GD20_ADDR,gOUT_Y_H);
+    zL=readByte(L3GD20_ADDR,gOUT_Z_L);
+    zH=readByte(L3GD20_ADDR,gOUT_Z_H);
+    
+    gX=(xH<<8) | (xL); // 16-bit 2's complement data
+    gY=(yH<<8) | (yL);
+    gZ=(zH<<8) | (zL);
+    
+    rorX=(float) gX * (17.5/1000.0);    // At 500dps sensitivity, L3GD20 returns 17.5/1000 dps per digit
+    rorY=(float) gY * (17.5/1000.0);
+    rorZ=(float) gZ * (17.5/1000.0);
+
+    pc.printf("Rate of rotation (dps):  X:%5.1f               Y:%5.1f              Z:%5.1f \n",rorX,rorY,rorZ);
+    //pc.printf("gX: %x           gY: %x          gZ: %x \n",gX,gY,gZ);
+    
+}
+
+int main()
+{
+    SensorState_t   state;
+    
+    initSensors();
+
+    while(1)
+    {
+    LPS331(state);
+    LSM303(&state);
+    L3GD20();
+    
+    pc.printf("\n");
+    wait(.5);
+    } 
+}
+    
+
+    

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 18 05:22:01 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8e73be2a2ac1
\ No newline at end of file