Roel Smits / Mbed 2 deprecated angle_test

test angle measurement LSM6DS3

Dependencies:   ESC IMUfilter LSM6DS3 mbed

main.cpp@3:78cf56b8eb21, 2018-02-10 (annotated)

Committer:
rsmits
Date:
Sat Feb 10 12:42:40 2018 +0000
Revision:
3:78cf56b8eb21
Parent:
2:405f8e1a01d3 
calibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsmits 0:a606f47629c3 1 #include "mbed.h"
rsmits 0:a606f47629c3 2 #include "esc.h"
rsmits 0:a606f47629c3 3 #include "LSM6DS3.h"
rsmits 0:a606f47629c3 4 #include "IMUfilter.h"
rsmits 0:a606f47629c3 5
rsmits 0:a606f47629c3 6 // refresh time. set to 500 for part 2 and 50 for part 4
rsmits 1:0e63617e1965 7 #define REFRESH_TIME_MS 100
rsmits 1:0e63617e1965 8 //Gravity at Earth's surface in m/s/s
rsmits 2:405f8e1a01d3 9 #define g0 9.812865328f
rsmits 2:405f8e1a01d3 10 //Convert from degrees to radians. radians = (degrees*pi)/180
rsmits 2:405f8e1a01d3 11 #define toRadians(x) (x * 0.01745329252f)
rsmits 2:405f8e1a01d3 12 //Convert from radians to degrees.
rsmits 2:405f8e1a01d3 13 #define toDegrees(x) (x * 57.2957795)
rsmits 3:78cf56b8eb21 14 //Number of samples to average.
rsmits 3:78cf56b8eb21 15 #define SAMPLES 4
rsmits 3:78cf56b8eb21 16 //Number of samples to be averaged for a null bias calculation
rsmits 3:78cf56b8eb21 17 //during calibration.
rsmits 3:78cf56b8eb21 18 #define CALIBRATION_SAMPLES 128
rsmits 3:78cf56b8eb21 19 //Sampling gyroscope at 200Hz.
rsmits 3:78cf56b8eb21 20 #define GYRO_RATE 0.005
rsmits 3:78cf56b8eb21 21 //Sampling accelerometer at 200Hz.
rsmits 3:78cf56b8eb21 22 #define ACC_RATE 0.005
rsmits 3:78cf56b8eb21 23 //Updating filter at 40Hz.
rsmits 3:78cf56b8eb21 24 #define FILTER_RATE 0.1
rsmits 0:a606f47629c3 25 const int LSM6DS3_ADDRESS = 0xD4;
rsmits 2:405f8e1a01d3 26 //const int LSM6DS3_ADDRESS = 0x69;
rsmits 1:0e63617e1965 27
rsmits 0:a606f47629c3 28 Serial pc(SERIAL_TX, SERIAL_RX);
rsmits 0:a606f47629c3 29 AnalogIn PRESSURE_SENSOR(PA_0);
rsmits 2:405f8e1a01d3 30 LSM6DS3 imu(PB_9, PB_8, LSM6DS3_ADDRESS);
rsmits 3:78cf56b8eb21 31 //At rest the gyroscope is centred around 0 and goes between about
rsmits 3:78cf56b8eb21 32 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
rsmits 3:78cf56b8eb21 33 //5/15 = 0.3 degrees/sec.
rsmits 3:78cf56b8eb21 34 IMUfilter imuFilter(FILTER_RATE, 0.3);
rsmits 3:78cf56b8eb21 35 Ticker filterTicker;
rsmits 2:405f8e1a01d3 36 Ticker ScreenTicker;
rsmits 0:a606f47629c3 37
rsmits 3:78cf56b8eb21 38 //Offsets for the gyroscope.
rsmits 3:78cf56b8eb21 39 //The readings we take when the gyroscope is stationary won't be 0, so we'll
rsmits 3:78cf56b8eb21 40 //average a set of readings we do get when the gyroscope is stationary and
rsmits 3:78cf56b8eb21 41 //take those away from subsequent readings to ensure the gyroscope is offset
rsmits 3:78cf56b8eb21 42 //or "biased" to 0.
rsmits 3:78cf56b8eb21 43 float w_xBias;
rsmits 3:78cf56b8eb21 44 float w_yBias;
rsmits 3:78cf56b8eb21 45 float w_zBias;
rsmits 3:78cf56b8eb21 46
rsmits 3:78cf56b8eb21 47 //Offsets for the accelerometer.
rsmits 3:78cf56b8eb21 48 //Same as with the gyroscope.
rsmits 3:78cf56b8eb21 49 float a_xBias;
rsmits 3:78cf56b8eb21 50 float a_yBias;
rsmits 3:78cf56b8eb21 51 float a_zBias;
rsmits 3:78cf56b8eb21 52
rsmits 3:78cf56b8eb21 53 //Accumulators used for oversampling and then averaging.
rsmits 3:78cf56b8eb21 54 volatile float a_xAccumulator = 0;
rsmits 3:78cf56b8eb21 55 volatile float a_yAccumulator = 0;
rsmits 3:78cf56b8eb21 56 volatile float a_zAccumulator = 0;
rsmits 3:78cf56b8eb21 57 volatile float w_xAccumulator = 0;
rsmits 3:78cf56b8eb21 58 volatile float w_yAccumulator = 0;
rsmits 3:78cf56b8eb21 59 volatile float w_zAccumulator = 0;
rsmits 3:78cf56b8eb21 60
rsmits 3:78cf56b8eb21 61 //Accelerometer and gyroscope readings for x, y, z axes.
rsmits 3:78cf56b8eb21 62 volatile float a_x;
rsmits 3:78cf56b8eb21 63 volatile float a_y;
rsmits 3:78cf56b8eb21 64 volatile float a_z;
rsmits 3:78cf56b8eb21 65 volatile float w_x;
rsmits 3:78cf56b8eb21 66 volatile float w_y;
rsmits 3:78cf56b8eb21 67 volatile float w_z;
rsmits 3:78cf56b8eb21 68
rsmits 3:78cf56b8eb21 69 //Buffer for accelerometer readings.
rsmits 3:78cf56b8eb21 70 float readings[3];
rsmits 3:78cf56b8eb21 71 //Number of accelerometer samples we're on.
rsmits 3:78cf56b8eb21 72 int accelerometerSamples = 0;
rsmits 3:78cf56b8eb21 73 //Number of gyroscope samples we're on.
rsmits 3:78cf56b8eb21 74 int gyroscopeSamples = 0;
rsmits 3:78cf56b8eb21 75
rsmits 3:78cf56b8eb21 76 /**
rsmits 3:78cf56b8eb21 77 * Prototypes
rsmits 3:78cf56b8eb21 78 */
rsmits 3:78cf56b8eb21 79 //Calculate the null bias.
rsmits 3:78cf56b8eb21 80 void calibrateAccelerometer(void);
rsmits 3:78cf56b8eb21 81 //Take a set of samples and average them.
rsmits 3:78cf56b8eb21 82 void sampleAccelerometer(void);
rsmits 3:78cf56b8eb21 83 //Calculate the null bias.
rsmits 3:78cf56b8eb21 84 void calibrateGyroscope(void);
rsmits 3:78cf56b8eb21 85 //Take a set of samples and average them.
rsmits 3:78cf56b8eb21 86 void sampleGyroscope(void);
rsmits 3:78cf56b8eb21 87 //Update the filter and calculate the Euler angles.
rsmits 3:78cf56b8eb21 88 void filter(void);
rsmits 3:78cf56b8eb21 89
rsmits 0:a606f47629c3 90 //Init Serial port and LSM6DS3 chip
rsmits 3:78cf56b8eb21 91 void setup_LSM6DS3()
rsmits 0:a606f47629c3 92 {
rsmits 0:a606f47629c3 93 // Use the begin() function to initialize the LSM9DS0 library.
rsmits 0:a606f47629c3 94 // You can either call it with no parameters (the easy way):
rsmits 0:a606f47629c3 95 // SLEEP
rsmits 0:a606f47629c3 96 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 97 // imu.G_POWER_DOWN, imu.A_POWER_DOWN);
rsmits 0:a606f47629c3 98 // LOWEST
rsmits 0:a606f47629c3 99 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 100 // imu.G_ODR_13_BW_0, imu.A_ODR_13);
rsmits 0:a606f47629c3 101 // HIGHEST
rsmits 0:a606f47629c3 102 // uint16_t status = imu.begin(imu.G_SCALE_2000DPS, imu.A_SCALE_8G,
rsmits 0:a606f47629c3 103 // imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 104
rsmits 0:a606f47629c3 105 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 106 imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 107
rsmits 0:a606f47629c3 108 //Make sure communication is working
rsmits 0:a606f47629c3 109 pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
rsmits 0:a606f47629c3 110 pc.printf("Should be 0x69\r\n");
rsmits 0:a606f47629c3 111 }
rsmits 2:405f8e1a01d3 112
rsmits 3:78cf56b8eb21 113 void screenUpdate(){
rsmits 3:78cf56b8eb21 114 pc.printf("%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),
rsmits 3:78cf56b8eb21 115 toDegrees(imuFilter.getPitch()),
rsmits 3:78cf56b8eb21 116 toDegrees(imuFilter.getYaw()));
rsmits 3:78cf56b8eb21 117 }
rsmits 3:78cf56b8eb21 118
rsmits 3:78cf56b8eb21 119 void calibrateAccelerometer(void) {
rsmits 3:78cf56b8eb21 120
rsmits 3:78cf56b8eb21 121 a_xAccumulator = 0;
rsmits 3:78cf56b8eb21 122 a_yAccumulator = 0;
rsmits 3:78cf56b8eb21 123 a_zAccumulator = 0;
rsmits 3:78cf56b8eb21 124
rsmits 3:78cf56b8eb21 125 //Take a number of readings and average them
rsmits 3:78cf56b8eb21 126 //to calculate the zero g offset.
rsmits 3:78cf56b8eb21 127 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
rsmits 3:78cf56b8eb21 128
rsmits 3:78cf56b8eb21 129 imu.readAccel();
rsmits 3:78cf56b8eb21 130 a_xAccumulator += imu.ax;
rsmits 3:78cf56b8eb21 131 a_yAccumulator += imu.ay;
rsmits 3:78cf56b8eb21 132 a_zAccumulator += imu.az;
rsmits 3:78cf56b8eb21 133
rsmits 3:78cf56b8eb21 134 wait(ACC_RATE);
rsmits 3:78cf56b8eb21 135 }
rsmits 3:78cf56b8eb21 136
rsmits 3:78cf56b8eb21 137 a_xAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 138 a_yAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 139 a_zAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 140
rsmits 3:78cf56b8eb21 141 a_xBias = a_xAccumulator;
rsmits 3:78cf56b8eb21 142 a_yBias = a_yAccumulator;
rsmits 3:78cf56b8eb21 143 // When laying on the table, default a_z is 1g
rsmits 3:78cf56b8eb21 144 a_zBias = (a_zAccumulator - 1.0f);
rsmits 3:78cf56b8eb21 145
rsmits 3:78cf56b8eb21 146 a_xAccumulator = 0;
rsmits 3:78cf56b8eb21 147 a_yAccumulator = 0;
rsmits 3:78cf56b8eb21 148 a_zAccumulator = 0;
rsmits 3:78cf56b8eb21 149 }
rsmits 3:78cf56b8eb21 150
rsmits 3:78cf56b8eb21 151 void sampleAccelerometer(void) {
rsmits 3:78cf56b8eb21 152
rsmits 3:78cf56b8eb21 153 //Have we taken enough samples?
rsmits 3:78cf56b8eb21 154 if (accelerometerSamples == SAMPLES) {
rsmits 3:78cf56b8eb21 155
rsmits 3:78cf56b8eb21 156 //Average the samples, remove the bias and convert to m/s/s.
rsmits 3:78cf56b8eb21 157 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * g0;
rsmits 3:78cf56b8eb21 158 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * g0;
rsmits 3:78cf56b8eb21 159 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * g0;
rsmits 3:78cf56b8eb21 160
rsmits 3:78cf56b8eb21 161 a_xAccumulator = 0;
rsmits 3:78cf56b8eb21 162 a_yAccumulator = 0;
rsmits 3:78cf56b8eb21 163 a_zAccumulator = 0;
rsmits 3:78cf56b8eb21 164 accelerometerSamples = 0;
rsmits 3:78cf56b8eb21 165
rsmits 3:78cf56b8eb21 166 } else {
rsmits 3:78cf56b8eb21 167 //Take another sample.
rsmits 3:78cf56b8eb21 168 imu.readAccel();
rsmits 3:78cf56b8eb21 169 a_xAccumulator += imu.ax;
rsmits 3:78cf56b8eb21 170 a_yAccumulator += imu.ay;
rsmits 3:78cf56b8eb21 171 a_zAccumulator += imu.az;
rsmits 3:78cf56b8eb21 172
rsmits 3:78cf56b8eb21 173 accelerometerSamples++;
rsmits 3:78cf56b8eb21 174 }
rsmits 3:78cf56b8eb21 175 }
rsmits 2:405f8e1a01d3 176
rsmits 3:78cf56b8eb21 177 void calibrateGyroscope(void) {
rsmits 3:78cf56b8eb21 178
rsmits 3:78cf56b8eb21 179 w_xAccumulator = 0;
rsmits 3:78cf56b8eb21 180 w_yAccumulator = 0;
rsmits 3:78cf56b8eb21 181 w_zAccumulator = 0;
rsmits 3:78cf56b8eb21 182
rsmits 3:78cf56b8eb21 183 //Take a number of readings and average them
rsmits 3:78cf56b8eb21 184 //to calculate the gyroscope bias offset.
rsmits 3:78cf56b8eb21 185 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
rsmits 3:78cf56b8eb21 186
rsmits 3:78cf56b8eb21 187 imu.readGyro();
rsmits 3:78cf56b8eb21 188 w_xAccumulator += imu.gx;
rsmits 3:78cf56b8eb21 189 w_yAccumulator += imu.gy;
rsmits 3:78cf56b8eb21 190 w_zAccumulator += imu.gz;
rsmits 3:78cf56b8eb21 191
rsmits 3:78cf56b8eb21 192 wait(GYRO_RATE);
rsmits 3:78cf56b8eb21 193 }
rsmits 3:78cf56b8eb21 194
rsmits 3:78cf56b8eb21 195 //Average the samples.
rsmits 3:78cf56b8eb21 196 w_xAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 197 w_yAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 198 w_zAccumulator /= CALIBRATION_SAMPLES;
rsmits 3:78cf56b8eb21 199
rsmits 3:78cf56b8eb21 200 w_xBias = w_xAccumulator;
rsmits 3:78cf56b8eb21 201 w_yBias = w_yAccumulator;
rsmits 3:78cf56b8eb21 202 w_zBias = w_zAccumulator;
rsmits 3:78cf56b8eb21 203
rsmits 3:78cf56b8eb21 204 w_xAccumulator = 0;
rsmits 3:78cf56b8eb21 205 w_yAccumulator = 0;
rsmits 3:78cf56b8eb21 206 w_zAccumulator = 0;
rsmits 3:78cf56b8eb21 207 }
rsmits 3:78cf56b8eb21 208
rsmits 3:78cf56b8eb21 209 void sampleGyroscope(void) {
rsmits 3:78cf56b8eb21 210
rsmits 3:78cf56b8eb21 211 //Have we taken enough samples?
rsmits 3:78cf56b8eb21 212 if (gyroscopeSamples == SAMPLES) {
rsmits 3:78cf56b8eb21 213
rsmits 3:78cf56b8eb21 214 //Average the samples, remove the bias, and calculate the angular
rsmits 3:78cf56b8eb21 215 //velocity in rad/s.
rsmits 3:78cf56b8eb21 216 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias));
rsmits 3:78cf56b8eb21 217 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias));
rsmits 3:78cf56b8eb21 218 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias));
rsmits 3:78cf56b8eb21 219
rsmits 3:78cf56b8eb21 220 w_xAccumulator = 0;
rsmits 3:78cf56b8eb21 221 w_yAccumulator = 0;
rsmits 3:78cf56b8eb21 222 w_zAccumulator = 0;
rsmits 3:78cf56b8eb21 223 gyroscopeSamples = 0;
rsmits 3:78cf56b8eb21 224
rsmits 3:78cf56b8eb21 225 } else {
rsmits 3:78cf56b8eb21 226 //Take another sample.
rsmits 3:78cf56b8eb21 227 imu.readGyro();
rsmits 3:78cf56b8eb21 228 w_xAccumulator += imu.gx;
rsmits 3:78cf56b8eb21 229 w_yAccumulator += imu.gy;
rsmits 3:78cf56b8eb21 230 w_zAccumulator += imu.gz;
rsmits 3:78cf56b8eb21 231
rsmits 3:78cf56b8eb21 232 gyroscopeSamples++;
rsmits 3:78cf56b8eb21 233 }
rsmits 3:78cf56b8eb21 234 }
rsmits 3:78cf56b8eb21 235
rsmits 3:78cf56b8eb21 236 void filter(void) {
rsmits 3:78cf56b8eb21 237
rsmits 3:78cf56b8eb21 238 //Update the filter variables.
rsmits 3:78cf56b8eb21 239 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
rsmits 3:78cf56b8eb21 240 //Calculate the new Euler angles.
rsmits 3:78cf56b8eb21 241 imuFilter.computeEuler();
rsmits 3:78cf56b8eb21 242
rsmits 2:405f8e1a01d3 243 }
rsmits 0:a606f47629c3 244
rsmits 0:a606f47629c3 245 int main()
rsmits 0:a606f47629c3 246 {
rsmits 0:a606f47629c3 247 bool started = false;
rsmits 0:a606f47629c3 248 while (true){
rsmits 0:a606f47629c3 249
rsmits 2:405f8e1a01d3 250 if(PRESSURE_SENSOR > 0.9f){
rsmits 0:a606f47629c3 251
rsmits 0:a606f47629c3 252 if (not started){
rsmits 3:78cf56b8eb21 253 setup_LSM6DS3(); //Setup sensor and Serial
rsmits 0:a606f47629c3 254 pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
rsmits 0:a606f47629c3 255 started=true;
rsmits 3:78cf56b8eb21 256
rsmits 3:78cf56b8eb21 257 calibrateAccelerometer();
rsmits 3:78cf56b8eb21 258 calibrateGyroscope();
rsmits 3:78cf56b8eb21 259 pc.printf("\r\n------ Calibrated -----------\r\n");
rsmits 3:78cf56b8eb21 260
rsmits 3:78cf56b8eb21 261 //Set up timers.
rsmits 3:78cf56b8eb21 262 filterTicker.attach(&filter, FILTER_RATE);
rsmits 3:78cf56b8eb21 263 ScreenTicker.attach(&screenUpdate, FILTER_RATE);
rsmits 0:a606f47629c3 264 }
rsmits 2:405f8e1a01d3 265
rsmits 3:78cf56b8eb21 266 wait(0.005);
rsmits 3:78cf56b8eb21 267 sampleAccelerometer();
rsmits 3:78cf56b8eb21 268 sampleGyroscope();
rsmits 3:78cf56b8eb21 269
rsmits 0:a606f47629c3 270 }
rsmits 0:a606f47629c3 271 else {
rsmits 0:a606f47629c3 272 started=false;
rsmits 1:0e63617e1965 273 imuFilter.reset();
rsmits 3:78cf56b8eb21 274 filterTicker.detach();
rsmits 2:405f8e1a01d3 275 ScreenTicker.detach();
rsmits 0:a606f47629c3 276 }
rsmits 0:a606f47629c3 277 }
rsmits 0:a606f47629c3 278 }