gyro.cpp

00001 #include "gyro.h"
00002 
00003 //DigitalOut myled(LED1);
00004 //Serial pc(USBTX, USBRX);    // tx, rx for USB debug printf to terminal console
00005 I2C i2c(p9, p10);          // LPC1768 I2C pin allocation
00006 //DigitalIn din(p23); // used as a test button
00007 
00008 // Globals
00009 int16_t const   Offset_mX=-40.0;
00010 int16_t const   Offset_mY=-115.0;
00011 float const     RadtoDeg=(180.0/3.141592654);
00012 
00013 MLX90614 IR_thermometer(&i2c);
00014 
00015 float get_temperature(void){
00016     float temp;
00017     IR_thermometer.getTemp(&temp);
00018     return temp;
00019 }
00020 
00021 char readByte(char address, char reg)
00022 // Reads one byte from an I2C address
00023 // Didn't bother to make a multi-byte version to read in X,Y,Z low/high series of registers because...
00024 // 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...
00025 {
00026     char result;
00027     
00028     i2c.start();
00029     i2c.write(address);         // Slave address with direction=write
00030     i2c.write(reg);             // Subaddress (register)
00031     
00032     i2c.start();                // Break transmission to change bus direction
00033     i2c.write(address + 1);     // Slave address with direction=read [bit0=1]
00034     
00035     result = i2c.read(0);
00036     i2c.stop();
00037     return (result);
00038     }
00039     
00040 void writeByte(char address, char reg,char value)
00041 // Sends 1 byte to an I2C address
00042 {    
00043     i2c.start();
00044     i2c.write(address);         // Slave address
00045     i2c.write(reg);             // Subaddress (register)
00046     i2c.write(value);
00047     i2c.stop();
00048     
00049     }
00050     
00051 void initSensors (void)
00052 // Switch on and set up the 3 on-board sensors
00053 {
00054     //pc.printf("--------------------------------------\n");
00055     //pc.printf("\nSTM MEMS eval board sensor init \n");
00056     
00057 #ifdef LSM303_on
00058     // LSM303DLHC Magnetic sensor
00059     //pc.printf("LSM303DLHC ping (should reply 0x48): %x \n",readByte(LSM303_m,mIRA_REG_M));
00060     writeByte(LSM303_m,mCRA_REG_M,0x94);     //switch on temperature sensor and set Output Data Rate to 30Hz
00061     writeByte(LSM303_m,mCRB_REG_M,0x20);     // Set the gain for +/- 1.3 Gauss full scale range
00062     writeByte(LSM303_m,mMR_REG_M,0x0);       // Continuous convertion mode
00063     
00064     // LSM303DLHC Accelerometer
00065     writeByte(LSM303_a,aCTRL_REG1_A ,0x37); // Set 25Hz ODR, everything else on
00066     writeByte(LSM303_a,aCTRL_REG4_A ,0x08); // Set full scale to +/- 2g sensitivity and high rez mode
00067 #endif
00068     
00069     //pc.printf("--------------------------------------\n \n");
00070     wait(2); // Wait for settings to stabilize
00071     }    
00072 
00073 void LSM303 (SensorState_t * state)
00074 // Magnetometer and accelerometer
00075 {
00076     char        xL, xH, yL, yH, zL, zH;
00077     int16_t     mX, mY, mZ,aX,aY,aZ;
00078     float       pitch,roll,faX,faY;
00079 
00080     xL=readByte(LSM303_m,mOUT_X_L_M);
00081     xH=readByte(LSM303_m,mOUT_X_H_M);
00082     yL=readByte(LSM303_m,mOUT_Y_L_M);
00083     yH=readByte(LSM303_m,mOUT_Y_H_M);
00084     zL=readByte(LSM303_m,mOUT_Z_L_M);
00085     zH=readByte(LSM303_m,mOUT_Z_H_M);
00086     
00087     mX=(xH<<8) | (xL); // 16-bit 2's complement data
00088     mY=(yH<<8) | (yL);
00089     mZ=(zH<<8) | (zL);
00090     
00091     //pc.printf("mX=%hd   %X          mY=%hd  %X      mZ=%hd  %X      \n",mX,mX,mY,mY,mZ,mZ);
00092     
00093     mX=mX-Offset_mX; // These are callibration co-efficients to deal with non-zero soft iron magnetic offset
00094     mY=mY-Offset_mY; 
00095     
00096     xL=readByte(LSM303_a,aOUT_X_L_A);
00097     xH=readByte(LSM303_a,aOUT_X_H_A);
00098     yL=readByte(LSM303_a,aOUT_Y_L_A);
00099     yH=readByte(LSM303_a,aOUT_Y_H_A);
00100     zL=readByte(LSM303_a,aOUT_Z_L_A);
00101     zH=readByte(LSM303_a,aOUT_Z_H_A);
00102 
00103     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.
00104     aY=(signed short) ( (yH<<8) | (yL) ) >> 4;
00105     aZ=(signed short) ( (zH<<8) | (zL) ) >> 4;
00106     
00107     //pc.printf("aX=%hd   %X          aY=%hd  %X      aZ=%hd  %X      \n",aX,aX,aY,aY,aZ,aZ);
00108     
00109     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.
00110     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)
00111     //faZ=((float) aZ) /2000.0; // Not used in a calc so comment out to avoid the compiler warning
00112         
00113     // Trigonometry derived from STM app note AN3192 and from WikiRobots
00114    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
00115    roll = asin(faY*2/cos(pitch));
00116    
00117   float xh = mX * cos(pitch) + mZ * sin(pitch);
00118   float yh = mX * sin(roll) * sin(pitch) + mY * cos(roll) - mZ * sin(roll) * cos(pitch);
00119   float zh = -mX * cos(roll) * sin(pitch) + mY * sin(roll) + mZ * cos(roll) * cos(pitch);
00120  
00121   float heading = atan2(yh, xh) * RadtoDeg; // Note use of atan2 rather than atan since better for working with quadrants
00122   if (yh < 0)
00123     heading=360+heading;
00124     
00125     state->heading=heading;
00126     state->pitch=pitch;
00127     state->roll=roll;
00128     state->aX = (float)aX;
00129     state->aY = (float)aY;
00130     state->aZ = (float)aZ;
00131     //5.1f 
00132     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n",roll*RadtoDeg,pitch*RadtoDeg,heading);
00133     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",aX,aY,aZ);    
00134     
00135 }
00136 
00137 //calculates the difference for acceleration in int16_t value  
00138 void calc_avrg_ac(Result_avrg* result,int samples){
00139     int i = 0;
00140     result -> aX = 0;
00141     result -> aY = 0;
00142     result -> aZ = 0;
00143     SensorState_t   state;
00144     for(i = 0;i<samples;i++){
00145         #ifdef LSM303_on
00146             LSM303(&state);
00147         #endif
00148         result -> aX += state.aX;
00149         result -> aY += state.aY;
00150         result -> aZ += state.aZ;
00151         //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",state.aX,state.aY,state.aZ);
00152            
00153         wait(0.01);
00154     }
00155     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",result->aX,result->aY,result->aZ);   
00156     result -> aX = result -> aX / samples;
00157     result -> aY = result -> aY / samples;
00158     result -> aZ = result -> aZ / samples;
00159     //pc.printf("rAcceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",result->aX,result->aY,result->aZ);   
00160 }
00161 
00162 //calculates the difference for orientation in float value
00163 void calc_avrg_or(Result_avrg* result,int samples){
00164     int i = 0;
00165     result -> x = 0;
00166     result -> y = 0;
00167     result -> z = 0;
00168     SensorState_t   state;
00169     for(i = 0;i<samples;i++){
00170         #ifdef LSM303_on
00171             LSM303(&state);
00172         #endif
00173         result -> x +=  state.roll*RadtoDeg;
00174         result -> y += state.pitch*RadtoDeg;
00175         result -> z += state.heading;
00176         //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
00177         wait(0.01);
00178     }
00179     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
00180     result -> x = result -> x / samples;
00181     result -> y = result -> y / samples;
00182     result -> z = result -> z / samples;
00183     //pc.printf("Orientation (deg):   rRot_X: %0.0f    rRot_Y: %0.0f     rRot_Z: %0.0f \n", result -> x ,result -> y,result -> z);
00184 }
00185 
00186 //gets the two results and saves the answer in r1 structure
00187 void calc_diff(Result_avrg* r1, Result_avrg* r2){
00188     r1 -> x = abs(r1->x - r2->x);
00189     r1 -> y = abs(r1->y - r2->y);
00190     r1 -> z = abs(r1->z - r2->z);
00191     
00192     r1 -> aX = abs(r1->aX - r2->aX);
00193     r1 -> aY = abs(r1->aY - r2->aY);
00194     r1 -> aZ = abs(r1->aZ - r2->aZ);
00195 }
00196 
00197 /*
00198 
00199 int main()
00200 {
00201     SensorState_t   state;
00202     Result_avrg result1;
00203     //Result_avrg result2;
00204     initSensors();
00205     //pc.baud(115200);
00206     calc_avrg_or(&result1,3);
00207     //calc_avrg_ac(&result1,3);
00208     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
00209     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",result1.aX,result1.aY,result1.aZ);   
00210     //calc_avrg_or(&result2,3);
00211     //calc_avrg_ac(&result2,3);
00212     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n", result2.x ,result2.y,result2.z);
00213     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",result2.aX,result2.aY,result2.aZ);
00214     //calc_diff(&result1,&result2);
00215     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n", result1.x ,result1.y,result1.z);
00216     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",result1.aX,result1.aY,result1.aZ);
00217 #ifdef LSM303_on
00218     //LSM303(&state);
00219 #endif
00220 
00221     //pc.printf("Orientation (deg):   Rot_X: %0.0f    Rot_Y: %0.0f     Rot_Z: %0.0f \n",state.roll*RadtoDeg,state.pitch*RadtoDeg,state.heading);
00222     //pc.printf("Acceleration (mg):   X: %5hd         Y: %5hd          Z: %5hd \n",state.aX,state.aY,state.aZ);  
00223     //pc.printf("\n");
00224     
00225 }
00226 */