Air Mouse Project
Dependencies: ADXL345_I2C IMUfilter ITG3200_lib USBDevice mbed
Fork of IMU by
main.cpp
- Committer:
- guqinchen
- Date:
- 2014-03-24
- Revision:
- 2:044740372a78
- Parent:
- 1:c8232c909f29
File content as of revision 2:044740372a78:
/** * IMU filter example. * * Calculate the roll, pitch and yaw angles. */ #include "IMUfilter.h" #include "ADXL345_I2C.h" #include "ITG3200.h" #include "USBMouse.h" #define MOVERATE 1 #define MAXMOVE 100 #define MOVETHRESHOLD 3 //Gravity at Earth's surface in m/s/s #define g0 9.812865328 //Number of samples to average. #define SAMPLES 2 //Number of samples to be averaged for a null bias calculation //during calibration. #define CALIBRATION_SAMPLES 32 //Convert from radians to degrees. #define toDegrees(x) (x * 57.2957795) //Convert from degrees to radians. #define toRadians(x) (x * 0.01745329252) //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). #define GYROSCOPE_GAIN (1 / 14.375) //Full scale resolution on the ADXL345 is 4mg/LSB. #define ACCELEROMETER_GAIN (0.000061035 * g0) //Sampling gyroscope at . #define GYRO_RATE 0.005 //Sampling accelerometer at #define ACC_RATE 0.005 #define FILTER_RATE 0.015 DigitalIn leftClick(p16); DigitalIn rightClick(p15); Serial pc(USBTX, USBRX); //At rest the gyroscope is centred around 0 and goes between about //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly //5/15 = 0.3 degrees/sec. IMUfilter imuFilter(FILTER_RATE, 0.3); ADXL345_I2C accelerometer(p28, p27); ITG3200 gyroscope(p28, p27); USBMouse mouse; Ticker accelerometerTicker; Ticker gyroscopeTicker; Ticker filterTicker; /** * IMU filter example. * * Calculate the roll, pitch and yaw angles. */ //Offsets for the gyroscope. //The readings we take when the gyroscope is stationary won't be 0, so we'll //average a set of readings we do get when the gyroscope is stationary and //take those away from subsequent readings to ensure the gyroscope is offset //or "biased" to 0. double w_xBias; double w_yBias; double w_zBias; //Offsets for the accelerometer. //Same as with the gyroscope. double a_xBias; double a_yBias; double a_zBias; //Accumulators used for oversampling and then averaging. volatile double a_xAccumulator = 0; volatile double a_yAccumulator = 0; volatile double a_zAccumulator = 0; volatile double w_xAccumulator = 0; volatile double w_yAccumulator = 0; volatile double w_zAccumulator = 0; //Accelerometer and gyroscope readings for x, y, z axes. volatile double a_x; volatile double a_y; volatile double a_z; volatile double w_x; volatile double w_y; volatile double w_z; //Buffer for accelerometer readings. int readings[3]; //Number of accelerometer samples we're on. int accelerometerSamples = 0; //Number of gyroscope samples we're on. int gyroscopeSamples = 0; /** * Prototypes */ //Set up the ADXL345 appropriately. void initializeAcceleromter(void); //Calculate the null bias. void calibrateAccelerometer(void); //Take a set of samples and average them. void sampleAccelerometer(void); //Set up the ITG3200 appropriately. void initializeGyroscope(void); //Calculate the null bias. void calibrateGyroscope(void); //Take a set of samples and average them. void sampleGyroscope(void); //Update the filter and calculate the Euler angles. void filter(void); void processMove(void); void processClick(void); void initializeAccelerometer(void) { //Go into standby mode to configure the device. accelerometer.setPowerControl(0x00); //Full resolution, +/-16g, 4mg/LSB. accelerometer.setDataFormatControl(0x0B); //200Hz data rate. accelerometer.setDataRate(ADXL345_1600HZ); //Measurement mode. accelerometer.setPowerControl(0x08); //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf wait_ms(22); } void initializeGyroscope(void) { //Low pass filter bandwidth of 42Hz. gyroscope.setLpBandwidth(LPFBW_42HZ); gyroscope.setSampleRateDivider(0); pc.printf("%d\n", gyroscope.getSampleRateDivider()); pc.printf("%d\n", gyroscope.getInternalSampleRate()); wait_ms(22); } void calibrateAccelerometer(void) { pc.printf("Calibrating Accelerometer\n"); a_xAccumulator = 0; a_yAccumulator = 0; a_zAccumulator = 0; //Take a number of readings and average them //to calculate the zero g offset. for (int i = 0; i < CALIBRATION_SAMPLES; i++) { accelerometer.getOutput(readings); a_xAccumulator += (int16_t) readings[0]; a_yAccumulator += (int16_t) readings[1]; a_zAccumulator += (int16_t) readings[2]; wait(ACC_RATE); } a_xAccumulator /= CALIBRATION_SAMPLES; a_yAccumulator /= CALIBRATION_SAMPLES; a_zAccumulator /= CALIBRATION_SAMPLES; //At 4mg/LSB, 250 LSBs is 1g. a_xBias = a_xAccumulator; a_yBias = a_yAccumulator; a_zBias = (a_zAccumulator - 981); a_xAccumulator = 0; a_yAccumulator = 0; a_zAccumulator = 0; pc.printf("Calibration Complete\n"); } void calibrateGyroscope(void) { pc.printf("Calibrating Gyro\n"); w_xAccumulator = 0; w_yAccumulator = 0; w_zAccumulator = 0; //Take a number of readings and average them //to calculate the gyroscope bias offset. for (int i = 0; i < CALIBRATION_SAMPLES; i++) { w_xAccumulator += gyroscope.getGyroX(); w_yAccumulator += gyroscope.getGyroY(); w_zAccumulator += gyroscope.getGyroZ(); wait(GYRO_RATE); } //Average the samples. w_xAccumulator /= CALIBRATION_SAMPLES; w_yAccumulator /= CALIBRATION_SAMPLES; w_zAccumulator /= CALIBRATION_SAMPLES; w_xBias = w_xAccumulator; w_yBias = w_yAccumulator; w_zBias = w_zAccumulator; w_xAccumulator = 0; w_yAccumulator = 0; w_zAccumulator = 0; pc.printf("Calibration Complete\n"); } void sampleAccelerometer(void) { //Have we taken enough samples? if (accelerometerSamples == SAMPLES) { //Average the samples, remove the bias, and calculate the acceleration //in m/s/s. a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; a_xAccumulator = 0; a_yAccumulator = 0; a_zAccumulator = 0; accelerometerSamples = 0; } else { //Take another sample. accelerometer.getOutput(readings); a_xAccumulator += (int16_t) readings[0]; a_yAccumulator += (int16_t) readings[1]; a_zAccumulator += (int16_t) readings[2]; accelerometerSamples++; //pc.printf("Sample Accl %d", accelerometerSamples); } } void sampleGyroscope(void) { //Have we taken enough samples? if (gyroscopeSamples == SAMPLES) { //Average the samples, remove the bias, and calculate the angular //velocity in rad/s. w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); w_xAccumulator = 0; w_yAccumulator = 0; w_zAccumulator = 0; gyroscopeSamples = 0; } else { //Take another sample. w_xAccumulator += gyroscope.getGyroX(); w_yAccumulator += gyroscope.getGyroY(); w_zAccumulator += gyroscope.getGyroZ(); gyroscopeSamples++; //pc.printf("Sample Gyro %d", gyroscopeSamples); } } void filter(void) { //Update the filter variables. imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); //Calculate the new Euler angles. imuFilter.computeEuler(); } void processClick() { static bool preRightClick = false; if (leftClick == 0) { mouse.press(MOUSE_LEFT); } else { mouse.release(MOUSE_LEFT); } // Right Mouse Click ___ Falling Edge Detection if (rightClick == 0 && preRightClick == false) { preRightClick = true; } else if (rightClick == 1 && preRightClick == true) { preRightClick = false; mouse.click(MOUSE_RIGHT); } } void processMove(void) { int16_t move_x, move_y; move_x = (int16_t)(-MOVERATE*toDegrees(imuFilter.getRoll())); move_y = (int16_t)(-MOVERATE*toDegrees(imuFilter.getPitch())); if (move_x <= MOVETHRESHOLD && move_x >= -MOVETHRESHOLD) move_x = 0; else if (move_x > MOVETHRESHOLD){ if (move_x > MAXMOVE+MOVETHRESHOLD) move_x = MAXMOVE; else move_x -=MOVETHRESHOLD; } else{ if (move_x < -MAXMOVE-MOVETHRESHOLD) move_x = -MAXMOVE; else move_x+=MOVETHRESHOLD; } if (move_y <= MOVETHRESHOLD && move_y >= -MOVETHRESHOLD) move_y = 0; else if (move_y > MOVETHRESHOLD){ if (move_y > MAXMOVE+MOVETHRESHOLD) move_y = MAXMOVE; else move_y -=MOVETHRESHOLD; } else{ if (move_y < -MAXMOVE-MOVETHRESHOLD) move_y = -MAXMOVE; else move_y+=MOVETHRESHOLD; } pc.printf("move_x = %d, move_ y = %d\n", move_x,move_y); mouse.move(move_x, move_y); } int main() { //pc.printf("Starting IMU filter test...\n"); //Initialize inertial sensors. initializeAccelerometer(); calibrateAccelerometer(); initializeGyroscope(); calibrateGyroscope(); leftClick.mode(PullUp); rightClick.mode(PullUp); //pc.printf("Initialized Successfully...\n\r"); //Set up timers. //Accelerometer data rate is 200Hz, so we'll sample at this speed. accelerometerTicker.attach(&sampleAccelerometer, GYRO_RATE); //Gyroscope data rate is 200Hz, so we'll sample at this speed. gyroscopeTicker.attach(&sampleGyroscope, ACC_RATE); //Update the filter variables at the correct rate. filterTicker.attach(&filter, FILTER_RATE); //pc.printf("Timers Setup...Entering Loop...\n\r"); while (1) { wait(0.01); pc.printf("angle_x = %f, angle_ y = %f\n",-toDegrees(imuFilter.getRoll()),-toDegrees(imuFilter.getPitch())); processClick(); processMove(); } }