Arnaud Suire / Labo_TRSE_Drone

ajout module_mouvement

Dependencies:   mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID

Fork of Labo_TRSE_Drone by HERBERT Nicolas

Service/Acc_Giro.h@37:e1ad11fe3fe4, 2014-02-26 (annotated)

Committer:
arnaudsuire
Date:
Wed Feb 26 08:46:22 2014 +0000
Revision:
37:e1ad11fe3fe4
arnaud pid acc giro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 37:e1ad11fe3fe4 1 #ifndef Acc_Giro_H
arnaudsuire 37:e1ad11fe3fe4 2 #define Acc_Giro_H
arnaudsuire 37:e1ad11fe3fe4 3
arnaudsuire 37:e1ad11fe3fe4 4 /**
arnaudsuire 37:e1ad11fe3fe4 5 * Includes
arnaudsuire 37:e1ad11fe3fe4 6 */
arnaudsuire 37:e1ad11fe3fe4 7 #include "mbed.h"
arnaudsuire 37:e1ad11fe3fe4 8 #include "ADXL345_I2C.h"
arnaudsuire 37:e1ad11fe3fe4 9 #include "ITG3200.h"
arnaudsuire 37:e1ad11fe3fe4 10 #include "IMUfilter.h"
arnaudsuire 37:e1ad11fe3fe4 11
arnaudsuire 37:e1ad11fe3fe4 12 /**
arnaudsuire 37:e1ad11fe3fe4 13 * Defines
arnaudsuire 37:e1ad11fe3fe4 14 */
arnaudsuire 37:e1ad11fe3fe4 15 //Gravity at Earth's surface in m/s/s
arnaudsuire 37:e1ad11fe3fe4 16 #define g0 9.812865328
arnaudsuire 37:e1ad11fe3fe4 17 //Number of samples to average.
arnaudsuire 37:e1ad11fe3fe4 18 #define SAMPLES 4
arnaudsuire 37:e1ad11fe3fe4 19 //Number of samples to be averaged for a null bias calculation
arnaudsuire 37:e1ad11fe3fe4 20 //during calibration.
arnaudsuire 37:e1ad11fe3fe4 21 #define CALIBRATION_SAMPLES 128
arnaudsuire 37:e1ad11fe3fe4 22 //Convert from radians to degrees.
arnaudsuire 37:e1ad11fe3fe4 23 #define toDegrees(x) (x * 57.2957795)
arnaudsuire 37:e1ad11fe3fe4 24 //Convert from degrees to radians.
arnaudsuire 37:e1ad11fe3fe4 25 #define toRadians(x) (x * 0.01745329252)
arnaudsuire 37:e1ad11fe3fe4 26 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
arnaudsuire 37:e1ad11fe3fe4 27 #define GYROSCOPE_GAIN (1 / 14.375)
arnaudsuire 37:e1ad11fe3fe4 28 //Full scale resolution on the ADXL345 is 4mg/LSB.
arnaudsuire 37:e1ad11fe3fe4 29 #define ACCELEROMETER_GAIN (0.004 * g0)
arnaudsuire 37:e1ad11fe3fe4 30 //Sampling gyroscope at 200Hz.
arnaudsuire 37:e1ad11fe3fe4 31 #define GYRO_RATE 0.002
arnaudsuire 37:e1ad11fe3fe4 32 //Sampling accelerometer at 200Hz.
arnaudsuire 37:e1ad11fe3fe4 33 #define ACC_RATE 0.002
arnaudsuire 37:e1ad11fe3fe4 34 //Updating filter at 100Hz.
arnaudsuire 37:e1ad11fe3fe4 35 #define FILTER_RATE 0.01
arnaudsuire 37:e1ad11fe3fe4 36 #define CORRECTION_MAGNITUDE 50
arnaudsuire 37:e1ad11fe3fe4 37
arnaudsuire 37:e1ad11fe3fe4 38 class Acc_Giro {
arnaudsuire 37:e1ad11fe3fe4 39
arnaudsuire 37:e1ad11fe3fe4 40 public:
arnaudsuire 37:e1ad11fe3fe4 41
arnaudsuire 37:e1ad11fe3fe4 42 //Constructor.
arnaudsuire 37:e1ad11fe3fe4 43 Acc_Giro();
arnaudsuire 37:e1ad11fe3fe4 44 //destructeur
arnaudsuire 37:e1ad11fe3fe4 45 // ~Acc_Giro();
arnaudsuire 37:e1ad11fe3fe4 46
arnaudsuire 37:e1ad11fe3fe4 47 //Set up the ADXL345 appropriately.
arnaudsuire 37:e1ad11fe3fe4 48 void initializeAccelerometer(void);
arnaudsuire 37:e1ad11fe3fe4 49 //Calculate the null bias.
arnaudsuire 37:e1ad11fe3fe4 50 void calibrateAccelerometer(void);
arnaudsuire 37:e1ad11fe3fe4 51 //Take a set of samples and average them.
arnaudsuire 37:e1ad11fe3fe4 52 void sampleAccelerometer(void);
arnaudsuire 37:e1ad11fe3fe4 53 //Set up the ITG3200 appropriately.
arnaudsuire 37:e1ad11fe3fe4 54 void initializeGyroscope(void);
arnaudsuire 37:e1ad11fe3fe4 55 //Calculate the null bias.
arnaudsuire 37:e1ad11fe3fe4 56 void calibrateGyroscope(void);
arnaudsuire 37:e1ad11fe3fe4 57 //Take a set of samples and average them.
arnaudsuire 37:e1ad11fe3fe4 58 void sampleGyroscope(void);
arnaudsuire 37:e1ad11fe3fe4 59 //Update the filter and calculate the Euler angles.
arnaudsuire 37:e1ad11fe3fe4 60 void filter(void);
arnaudsuire 37:e1ad11fe3fe4 61 void GetI2CAddress();
arnaudsuire 37:e1ad11fe3fe4 62 void dataSender(void);
arnaudsuire 37:e1ad11fe3fe4 63
arnaudsuire 37:e1ad11fe3fe4 64 //membre classe
arnaudsuire 37:e1ad11fe3fe4 65 IMUfilter *imuFilter;
arnaudsuire 37:e1ad11fe3fe4 66 ADXL345_I2C *accelerometer;
arnaudsuire 37:e1ad11fe3fe4 67 I2C *i2c;
arnaudsuire 37:e1ad11fe3fe4 68 ITG3200 *gyroscope;
arnaudsuire 37:e1ad11fe3fe4 69 Ticker accelerometerTicker;
arnaudsuire 37:e1ad11fe3fe4 70 Ticker gyroscopeTicker;
arnaudsuire 37:e1ad11fe3fe4 71 Ticker filterTicker;
arnaudsuire 37:e1ad11fe3fe4 72 Ticker dataTicker;
arnaudsuire 37:e1ad11fe3fe4 73 Ticker algorithmTicker;
arnaudsuire 37:e1ad11fe3fe4 74
arnaudsuire 37:e1ad11fe3fe4 75
arnaudsuire 37:e1ad11fe3fe4 76
arnaudsuire 37:e1ad11fe3fe4 77 int calibrate;
arnaudsuire 37:e1ad11fe3fe4 78 int calibrated;
arnaudsuire 37:e1ad11fe3fe4 79 int start;
arnaudsuire 37:e1ad11fe3fe4 80 int started;
arnaudsuire 37:e1ad11fe3fe4 81 int alg_enable;
arnaudsuire 37:e1ad11fe3fe4 82
arnaudsuire 37:e1ad11fe3fe4 83 char data[128];
arnaudsuire 37:e1ad11fe3fe4 84 //Offsets for the gyroscope.
arnaudsuire 37:e1ad11fe3fe4 85 //The readings we take when the gyroscope is stationary won't be 0, so we'll
arnaudsuire 37:e1ad11fe3fe4 86 //average a set of readings we do get when the gyroscope is stationary and
arnaudsuire 37:e1ad11fe3fe4 87 //take those away from subsequent readings to ensure the gyroscope is offset
arnaudsuire 37:e1ad11fe3fe4 88 //or "biased" to 0.
arnaudsuire 37:e1ad11fe3fe4 89 double w_xBias;
arnaudsuire 37:e1ad11fe3fe4 90 double w_yBias;
arnaudsuire 37:e1ad11fe3fe4 91 double w_zBias;
arnaudsuire 37:e1ad11fe3fe4 92
arnaudsuire 37:e1ad11fe3fe4 93 //Offsets for the accelerometer.
arnaudsuire 37:e1ad11fe3fe4 94 //Same as with the gyroscope.
arnaudsuire 37:e1ad11fe3fe4 95 double a_xBias;
arnaudsuire 37:e1ad11fe3fe4 96 double a_yBias;
arnaudsuire 37:e1ad11fe3fe4 97 double a_zBias;
arnaudsuire 37:e1ad11fe3fe4 98
arnaudsuire 37:e1ad11fe3fe4 99 //Accumulators used for oversampling and then averaging.
arnaudsuire 37:e1ad11fe3fe4 100 volatile double a_xAccumulator;
arnaudsuire 37:e1ad11fe3fe4 101 volatile double a_yAccumulator;
arnaudsuire 37:e1ad11fe3fe4 102 volatile double a_zAccumulator;
arnaudsuire 37:e1ad11fe3fe4 103 volatile double w_xAccumulator;
arnaudsuire 37:e1ad11fe3fe4 104 volatile double w_yAccumulator;
arnaudsuire 37:e1ad11fe3fe4 105 volatile double w_zAccumulator;
arnaudsuire 37:e1ad11fe3fe4 106
arnaudsuire 37:e1ad11fe3fe4 107 //Accelerometer and gyroscope readings for x, y, z axes.
arnaudsuire 37:e1ad11fe3fe4 108 volatile double a_x;
arnaudsuire 37:e1ad11fe3fe4 109 volatile double a_y;
arnaudsuire 37:e1ad11fe3fe4 110 volatile double a_z;
arnaudsuire 37:e1ad11fe3fe4 111 volatile double w_x;
arnaudsuire 37:e1ad11fe3fe4 112 volatile double w_y;
arnaudsuire 37:e1ad11fe3fe4 113 volatile double w_z;
arnaudsuire 37:e1ad11fe3fe4 114
arnaudsuire 37:e1ad11fe3fe4 115 //Buffer for accelerometer readings.
arnaudsuire 37:e1ad11fe3fe4 116 int readings[3];
arnaudsuire 37:e1ad11fe3fe4 117 //Number of accelerometer samples we're on.
arnaudsuire 37:e1ad11fe3fe4 118 int accelerometerSamples;
arnaudsuire 37:e1ad11fe3fe4 119 //Number of gyroscope samples we're on.
arnaudsuire 37:e1ad11fe3fe4 120 int gyroscopeSamples;
arnaudsuire 37:e1ad11fe3fe4 121
arnaudsuire 37:e1ad11fe3fe4 122 };
arnaudsuire 37:e1ad11fe3fe4 123
arnaudsuire 37:e1ad11fe3fe4 124
arnaudsuire 37:e1ad11fe3fe4 125
arnaudsuire 37:e1ad11fe3fe4 126 #endif /* PID_H */