Mitchell Pang / Mbed 2 deprecated RC_boat

RC Boat using a C# GUI over XBee communication.

Dependencies:   MODSERIAL LSM9DS0 Motordriver PID mbed-rtos mbed

RC Boat Wiki Page

main_works_velocity_4_21_15.txt@10:6cefe0eae1b1, 2015-05-01 (annotated)

Committer:
mitchpang
Date:
Fri May 01 00:43:19 2015 +0000
Revision:
10:6cefe0eae1b1
Parent:
0:58395e885409 
Working, we think...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mitchpang 0:58395e885409 1 #include "mbed.h"
mitchpang 0:58395e885409 2 #include "LSM9DS0.h"
mitchpang 0:58395e885409 3 #include "rtos.h"
mitchpang 0:58395e885409 4 #include "PID.h"
mitchpang 0:58395e885409 5 #include "IMUfilter.h"
mitchpang 0:58395e885409 6
mitchpang 0:58395e885409 7 //Gravity at Earth's surface in m/s/s
mitchpang 0:58395e885409 8 #define g0 9.812865328
mitchpang 0:58395e885409 9 //Number of samples to average.
mitchpang 0:58395e885409 10 #define SAMPLES 4
mitchpang 0:58395e885409 11 //Number of samples to be averaged for a null bias calculation
mitchpang 0:58395e885409 12 //during calibration.
mitchpang 0:58395e885409 13 #define CALIBRATION_SAMPLES 1024
mitchpang 0:58395e885409 14 //Convert from radians to degrees.
mitchpang 0:58395e885409 15 #define toDegrees(x) (x * 57.2957795)
mitchpang 0:58395e885409 16 //Convert from degrees to radians.
mitchpang 0:58395e885409 17 #define toRadians(x) (x * 0.01745329252)
mitchpang 0:58395e885409 18 //LSM9DS0 gyroscope sensitivity is 8.75 (millidegrees/sec)/digit when set to +-2G
mitchpang 0:58395e885409 19 #define GYROSCOPE_GAIN (0.007476806640625)
mitchpang 0:58395e885409 20 //Full scale resolution on the accelerometer is 0.0001220703125g/LSB.
mitchpang 0:58395e885409 21 #define ACCELEROMETER_GAIN (0.00006103515625 * g0)
mitchpang 0:58395e885409 22 //Sampling gyroscope at 100Hz.
mitchpang 0:58395e885409 23 #define GYRO_RATE 0.01
mitchpang 0:58395e885409 24 //Sampling accelerometer at 200Hz.
mitchpang 0:58395e885409 25 #define ACC_RATE 0.005
mitchpang 0:58395e885409 26 //Updating filter at 40Hz.
mitchpang 0:58395e885409 27 #define FILTER_RATE 0.1
mitchpang 0:58395e885409 28 // SDO_XM and SDO_G are both grounded, so our addresses are:
mitchpang 0:58395e885409 29 #define LSM9DS0_XM 0x1E // Would be 0x1E if SDO_XM is LOW
mitchpang 0:58395e885409 30 #define LSM9DS0_G 0x6A // Would be 0x6A if SDO_G is LOW
mitchpang 0:58395e885409 31
mitchpang 0:58395e885409 32 Serial pc(USBTX, USBRX);
mitchpang 0:58395e885409 33 //At rest the gyroscope is centred around 0 and goes between about
mitchpang 0:58395e885409 34 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
mitchpang 0:58395e885409 35 //5/15 = 0.3 degrees/sec.
mitchpang 0:58395e885409 36 IMUfilter imuFilter(FILTER_RATE, 0.3);
mitchpang 0:58395e885409 37 LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
mitchpang 0:58395e885409 38 DigitalIn DReady(p23);
mitchpang 0:58395e885409 39 Ticker accelerometerTicker;
mitchpang 0:58395e885409 40 Ticker gyroscopeTicker;
mitchpang 0:58395e885409 41 Ticker filterTicker;
mitchpang 0:58395e885409 42 Mutex pcMutex;
mitchpang 0:58395e885409 43 DigitalOut led1(LED1);
mitchpang 0:58395e885409 44 DigitalOut led2(LED2);
mitchpang 0:58395e885409 45 //Offsets for the gyroscope.
mitchpang 0:58395e885409 46 //The readings we take when the gyroscope is stationary won't be 0, so we'll
mitchpang 0:58395e885409 47 //average a set of readings we do get when the gyroscope is stationary and
mitchpang 0:58395e885409 48 //take those away from subsequent readings to ensure the gyroscope is offset
mitchpang 0:58395e885409 49 //or "biased" to 0.
mitchpang 0:58395e885409 50 double w_xBias;
mitchpang 0:58395e885409 51 double w_yBias;
mitchpang 0:58395e885409 52 double w_zBias;
mitchpang 0:58395e885409 53
mitchpang 0:58395e885409 54 //Offsets for the accelerometer.
mitchpang 0:58395e885409 55 //Same as with the gyroscope.
mitchpang 0:58395e885409 56 double a_xBias;
mitchpang 0:58395e885409 57 double a_yBias;
mitchpang 0:58395e885409 58 double a_zBias;
mitchpang 0:58395e885409 59
mitchpang 0:58395e885409 60 //Accumulators used for oversampling and then averaging.
mitchpang 0:58395e885409 61 double a_xAccumulator = 0;
mitchpang 0:58395e885409 62 double a_yAccumulator = 0;
mitchpang 0:58395e885409 63 double a_zAccumulator = 0;
mitchpang 0:58395e885409 64 double w_xAccumulator = 0;
mitchpang 0:58395e885409 65 double w_yAccumulator = 0;
mitchpang 0:58395e885409 66 double w_zAccumulator = 0;
mitchpang 0:58395e885409 67
mitchpang 0:58395e885409 68 //Accelerometer and gyroscope readings for x, y, z axes.
mitchpang 0:58395e885409 69 double a_x;
mitchpang 0:58395e885409 70 double a_y;
mitchpang 0:58395e885409 71 double a_z;
mitchpang 0:58395e885409 72 double w_x;
mitchpang 0:58395e885409 73 double w_y;
mitchpang 0:58395e885409 74 double w_z;
mitchpang 0:58395e885409 75
mitchpang 0:58395e885409 76 //Previous Acceleromerter and gyroscope readings for integration
mitchpang 0:58395e885409 77 double last_a_x;
mitchpang 0:58395e885409 78 double last_a_y;
mitchpang 0:58395e885409 79 double last_a_z;
mitchpang 0:58395e885409 80
mitchpang 0:58395e885409 81 //Buffer for accelerometer readings.
mitchpang 0:58395e885409 82 int readings[3];
mitchpang 0:58395e885409 83 //Number of accelerometer samples we're on.
mitchpang 0:58395e885409 84 int accelerometerSamples = 0;
mitchpang 0:58395e885409 85 //Number of gyroscope samples we're on.
mitchpang 0:58395e885409 86 int gyroscopeSamples = 0;
mitchpang 0:58395e885409 87
mitchpang 0:58395e885409 88 //current readings of linear velocity
mitchpang 0:58395e885409 89 double v_x = 0;
mitchpang 0:58395e885409 90 double v_y = 0;
mitchpang 0:58395e885409 91 double v_z = 0;
mitchpang 0:58395e885409 92
mitchpang 0:58395e885409 93 /**
mitchpang 0:58395e885409 94 * Prototypes
mitchpang 0:58395e885409 95 */
mitchpang 0:58395e885409 96 //Calculate the null bias.
mitchpang 0:58395e885409 97 void calibrateAccelerometer(void);
mitchpang 0:58395e885409 98 //Take a set of samples and average them.
mitchpang 0:58395e885409 99 void sampleAccelerometer(void);
mitchpang 0:58395e885409 100 //Calculate the null bias.
mitchpang 0:58395e885409 101 void calibrateGyroscope(void);
mitchpang 0:58395e885409 102 //Take a set of samples and average them.
mitchpang 0:58395e885409 103 void sampleGyroscope(void);
mitchpang 0:58395e885409 104 //Update the filter and calculate the Euler angles.
mitchpang 0:58395e885409 105 void filter(void);
mitchpang 0:58395e885409 106
mitchpang 0:58395e885409 107
mitchpang 0:58395e885409 108
mitchpang 0:58395e885409 109 void sampleAccelerometer(void const *args) {
mitchpang 0:58395e885409 110 while(1) {
mitchpang 0:58395e885409 111 //Have we taken enough samples?
mitchpang 0:58395e885409 112 if (accelerometerSamples == SAMPLES) {
mitchpang 0:58395e885409 113
mitchpang 0:58395e885409 114 //Average the samples, remove the bias, and calculate the acceleration
mitchpang 0:58395e885409 115 //in m/s/s.
mitchpang 0:58395e885409 116 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 117 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 118 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 119
mitchpang 0:58395e885409 120 a_xAccumulator = 0;
mitchpang 0:58395e885409 121 a_yAccumulator = 0;
mitchpang 0:58395e885409 122 a_zAccumulator = 0;
mitchpang 0:58395e885409 123 accelerometerSamples = 0;
mitchpang 0:58395e885409 124 pcMutex.lock();
mitchpang 0:58395e885409 125 pc.printf("a_x: %f a_y: %f a_z: %f \n",a_x,a_y,a_z);
mitchpang 0:58395e885409 126 pcMutex.unlock();
mitchpang 0:58395e885409 127
mitchpang 0:58395e885409 128 // integrate to get velocity. make sure to subtract gravity downwards!
mitchpang 0:58395e885409 129
mitchpang 0:58395e885409 130 v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 131 v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 132 v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 133
mitchpang 0:58395e885409 134 last_a_x = a_x;
mitchpang 0:58395e885409 135 last_a_y = a_y;
mitchpang 0:58395e885409 136 last_a_z = a_z;
mitchpang 0:58395e885409 137 pcMutex.lock();
mitchpang 0:58395e885409 138 pc.printf("v_x: %f v_y: %f v_z: %f \n",v_x,v_y,v_z);
mitchpang 0:58395e885409 139 pcMutex.unlock();
mitchpang 0:58395e885409 140
mitchpang 0:58395e885409 141 } else {
mitchpang 0:58395e885409 142 //Take another sample.
mitchpang 0:58395e885409 143 imu.readAccel();
mitchpang 0:58395e885409 144 pcMutex.lock();
mitchpang 0:58395e885409 145 pc.printf("A: ");
mitchpang 0:58395e885409 146 pc.printf("%d", imu.ax);
mitchpang 0:58395e885409 147 pc.printf(", ");
mitchpang 0:58395e885409 148 pc.printf("%d",imu.ay);
mitchpang 0:58395e885409 149 pc.printf(", ");
mitchpang 0:58395e885409 150 pc.printf("%d\n",imu.az);
mitchpang 0:58395e885409 151 pcMutex.unlock();
mitchpang 0:58395e885409 152
mitchpang 0:58395e885409 153 a_xAccumulator += imu.ax;
mitchpang 0:58395e885409 154 a_yAccumulator += imu.ay;
mitchpang 0:58395e885409 155 a_zAccumulator += imu.az;
mitchpang 0:58395e885409 156
mitchpang 0:58395e885409 157 accelerometerSamples++;
mitchpang 0:58395e885409 158 Thread::wait(ACC_RATE * 1000);
mitchpang 0:58395e885409 159 }
mitchpang 0:58395e885409 160
mitchpang 0:58395e885409 161
mitchpang 0:58395e885409 162 }
mitchpang 0:58395e885409 163 }
mitchpang 0:58395e885409 164
mitchpang 0:58395e885409 165 void calibrateAccelerometer(void) {
mitchpang 0:58395e885409 166
mitchpang 0:58395e885409 167 led1 = 1;
mitchpang 0:58395e885409 168
mitchpang 0:58395e885409 169 a_xAccumulator = 0;
mitchpang 0:58395e885409 170 a_yAccumulator = 0;
mitchpang 0:58395e885409 171 a_zAccumulator = 0;
mitchpang 0:58395e885409 172
mitchpang 0:58395e885409 173
mitchpang 0:58395e885409 174 //Take a number of readings and average them
mitchpang 0:58395e885409 175 //to calculate the zero g offset.
mitchpang 0:58395e885409 176 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 0:58395e885409 177
mitchpang 0:58395e885409 178 imu.readAccel();
mitchpang 0:58395e885409 179
mitchpang 0:58395e885409 180 a_xAccumulator += (double) imu.ax;
mitchpang 0:58395e885409 181 a_yAccumulator += (double) imu.ay;
mitchpang 0:58395e885409 182 a_zAccumulator += (double) imu.az;
mitchpang 0:58395e885409 183
mitchpang 0:58395e885409 184 wait(ACC_RATE);
mitchpang 0:58395e885409 185
mitchpang 0:58395e885409 186 }
mitchpang 0:58395e885409 187
mitchpang 0:58395e885409 188 a_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 189 a_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 190 a_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 191
mitchpang 0:58395e885409 192 //At 4mg/LSB, 250 LSBs is 1g.
mitchpang 0:58395e885409 193 a_xBias = a_xAccumulator;
mitchpang 0:58395e885409 194 a_yBias = a_yAccumulator;
mitchpang 0:58395e885409 195 //a_zBias = (a_zAccumulator - (1/0.00006103515625));
mitchpang 0:58395e885409 196 a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
mitchpang 0:58395e885409 197
mitchpang 0:58395e885409 198 pcMutex.lock();
mitchpang 0:58395e885409 199 pc.printf("A_Bias: ");
mitchpang 0:58395e885409 200 pc.printf("%f", a_xBias);
mitchpang 0:58395e885409 201 pc.printf(", ");
mitchpang 0:58395e885409 202 pc.printf("%f",a_yBias);
mitchpang 0:58395e885409 203 pc.printf(", ");
mitchpang 0:58395e885409 204 pc.printf("%f\n",a_zBias);
mitchpang 0:58395e885409 205 pcMutex.unlock();
mitchpang 0:58395e885409 206
mitchpang 0:58395e885409 207 a_xAccumulator = 0;
mitchpang 0:58395e885409 208 a_yAccumulator = 0;
mitchpang 0:58395e885409 209 a_zAccumulator = 0;
mitchpang 0:58395e885409 210 pc.printf("done calibrating accel\n");
mitchpang 0:58395e885409 211 led1 = 0;
mitchpang 0:58395e885409 212 }
mitchpang 0:58395e885409 213
mitchpang 0:58395e885409 214
mitchpang 0:58395e885409 215 void calibrateGyroscope(void) {
mitchpang 0:58395e885409 216 led2 = 1;
mitchpang 0:58395e885409 217 w_xAccumulator = 0;
mitchpang 0:58395e885409 218 w_yAccumulator = 0;
mitchpang 0:58395e885409 219 w_zAccumulator = 0;
mitchpang 0:58395e885409 220
mitchpang 0:58395e885409 221 //Take a number of readings and average them
mitchpang 0:58395e885409 222 //to calculate the gyroscope bias offset.
mitchpang 0:58395e885409 223 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 0:58395e885409 224 imu.readGyro();
mitchpang 0:58395e885409 225 w_xAccumulator += (double) imu.gx;
mitchpang 0:58395e885409 226 w_yAccumulator += (double) imu.gy;
mitchpang 0:58395e885409 227 w_zAccumulator += (double) imu.gz;
mitchpang 0:58395e885409 228 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 229
mitchpang 0:58395e885409 230 }
mitchpang 0:58395e885409 231
mitchpang 0:58395e885409 232 //Average the samples.
mitchpang 0:58395e885409 233 w_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 234 w_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 235 w_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 236
mitchpang 0:58395e885409 237 w_xBias = w_xAccumulator;
mitchpang 0:58395e885409 238 w_yBias = w_yAccumulator;
mitchpang 0:58395e885409 239 w_zBias = w_zAccumulator;
mitchpang 0:58395e885409 240
mitchpang 0:58395e885409 241 pcMutex.lock();
mitchpang 0:58395e885409 242 pc.printf("G_Bias: ");
mitchpang 0:58395e885409 243 pc.printf("%f", w_xBias);
mitchpang 0:58395e885409 244 pc.printf(", ");
mitchpang 0:58395e885409 245 pc.printf("%f",w_yBias);
mitchpang 0:58395e885409 246 pc.printf(", ");
mitchpang 0:58395e885409 247 pc.printf("%f\n",w_zBias);
mitchpang 0:58395e885409 248 pcMutex.unlock();
mitchpang 0:58395e885409 249
mitchpang 0:58395e885409 250 w_xAccumulator = 0;
mitchpang 0:58395e885409 251 w_yAccumulator = 0;
mitchpang 0:58395e885409 252 w_zAccumulator = 0;
mitchpang 0:58395e885409 253 pc.printf("done calibrating gyro\n");
mitchpang 0:58395e885409 254 led2 = 0;
mitchpang 0:58395e885409 255 }
mitchpang 0:58395e885409 256
mitchpang 0:58395e885409 257 void sampleGyroscope(void const *args) {
mitchpang 0:58395e885409 258 while(1){
mitchpang 0:58395e885409 259 //Have we taken enough samples?
mitchpang 0:58395e885409 260 if (gyroscopeSamples == SAMPLES) {
mitchpang 0:58395e885409 261
mitchpang 0:58395e885409 262 //Average the samples, remove the bias, and calculate the angular
mitchpang 0:58395e885409 263 //velocity in deg/s.
mitchpang 0:58395e885409 264 w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 265 w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 266 w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 267 pcMutex.lock();
mitchpang 0:58395e885409 268 pc.printf("w_x: %f w_y: %f w_z: %f \n",w_x,w_y,w_z);
mitchpang 0:58395e885409 269 pcMutex.unlock();
mitchpang 0:58395e885409 270 w_xAccumulator = 0;
mitchpang 0:58395e885409 271 w_yAccumulator = 0;
mitchpang 0:58395e885409 272 w_zAccumulator = 0;
mitchpang 0:58395e885409 273 gyroscopeSamples = 0;
mitchpang 0:58395e885409 274
mitchpang 0:58395e885409 275 } else {
mitchpang 0:58395e885409 276 imu.readGyro();
mitchpang 0:58395e885409 277 pcMutex.lock();
mitchpang 0:58395e885409 278 pc.printf("G: ");
mitchpang 0:58395e885409 279 pc.printf("%d", imu.gx);
mitchpang 0:58395e885409 280 pc.printf(", ");
mitchpang 0:58395e885409 281 pc.printf("%d",imu.gy);
mitchpang 0:58395e885409 282 pc.printf(", ");
mitchpang 0:58395e885409 283 pc.printf("%d\n",imu.gz);
mitchpang 0:58395e885409 284 pcMutex.unlock();
mitchpang 0:58395e885409 285
mitchpang 0:58395e885409 286 w_xAccumulator += imu.gx;
mitchpang 0:58395e885409 287 w_yAccumulator += imu.gy;
mitchpang 0:58395e885409 288 w_zAccumulator += imu.gz;
mitchpang 0:58395e885409 289
mitchpang 0:58395e885409 290 gyroscopeSamples++;
mitchpang 0:58395e885409 291 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 292 }
mitchpang 0:58395e885409 293
mitchpang 0:58395e885409 294 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 295 }
mitchpang 0:58395e885409 296 }
mitchpang 0:58395e885409 297
mitchpang 0:58395e885409 298 void filter(void const *args) {
mitchpang 0:58395e885409 299 while(1){
mitchpang 0:58395e885409 300 //Update the filter variables.
mitchpang 0:58395e885409 301 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
mitchpang 0:58395e885409 302 //Calculate the new Euler angles.
mitchpang 0:58395e885409 303 imuFilter.computeEuler();
mitchpang 0:58395e885409 304 Thread::wait(FILTER_RATE);
mitchpang 0:58395e885409 305 }
mitchpang 0:58395e885409 306 }
mitchpang 0:58395e885409 307
mitchpang 0:58395e885409 308
mitchpang 0:58395e885409 309 void setup()
mitchpang 0:58395e885409 310 {
mitchpang 0:58395e885409 311 pc.baud(9600); // Start serial at 115200 bps
mitchpang 0:58395e885409 312 // Use the begin() function to initialize the LSM9DS0 library.
mitchpang 0:58395e885409 313 // You can either call it with no parameters (the easy way):
mitchpang 0:58395e885409 314 //uint16_t status = dof.begin();
mitchpang 0:58395e885409 315
mitchpang 0:58395e885409 316 //Or call it with declarations for sensor scales and data rates:
mitchpang 0:58395e885409 317 //uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
mitchpang 0:58395e885409 318 // dof.A_SCALE_2G, dof.M_SCALE_2GS);
mitchpang 0:58395e885409 319
mitchpang 0:58395e885409 320 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
mitchpang 0:58395e885409 321 imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
mitchpang 0:58395e885409 322
mitchpang 0:58395e885409 323 // begin() returns a 16-bit value which includes both the gyro
mitchpang 0:58395e885409 324 // and accelerometers WHO_AM_I response. You can check this to
mitchpang 0:58395e885409 325 // make sure communication was successful.
mitchpang 0:58395e885409 326 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
mitchpang 0:58395e885409 327 pc.printf("%x\n",status);
mitchpang 0:58395e885409 328 pc.printf("Should be 0x49D4\n");
mitchpang 0:58395e885409 329 pc.printf("\n");
mitchpang 0:58395e885409 330 }
mitchpang 0:58395e885409 331
mitchpang 0:58395e885409 332
mitchpang 0:58395e885409 333 int main() {
mitchpang 0:58395e885409 334
mitchpang 0:58395e885409 335 pc.printf("Starting IMU filter test...\n");
mitchpang 0:58395e885409 336 setup();
mitchpang 0:58395e885409 337
mitchpang 0:58395e885409 338
mitchpang 0:58395e885409 339 //Initialize inertial sensors.
mitchpang 0:58395e885409 340 calibrateAccelerometer();
mitchpang 0:58395e885409 341 calibrateGyroscope();
mitchpang 0:58395e885409 342
mitchpang 0:58395e885409 343 Thread accelThread(sampleAccelerometer);
mitchpang 0:58395e885409 344 Thread gyroThread(sampleGyroscope);
mitchpang 0:58395e885409 345 //Thread filterThread(filter);
mitchpang 0:58395e885409 346 /*
mitchpang 0:58395e885409 347 pcMutex.lock();
mitchpang 0:58395e885409 348 pc.printf("done attaching tickers\n\n");
mitchpang 0:58395e885409 349 pcMutex.unlock();
mitchpang 0:58395e885409 350 */
mitchpang 0:58395e885409 351 while (1) {
mitchpang 0:58395e885409 352 //pc.printf("in loop\n");
mitchpang 0:58395e885409 353 Thread::wait(2000);
mitchpang 0:58395e885409 354 /*
mitchpang 0:58395e885409 355 pcMutex.lock();
mitchpang 0:58395e885409 356 pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
mitchpang 0:58395e885409 357 toDegrees(imuFilter.getPitch()),
mitchpang 0:58395e885409 358 toDegrees(imuFilter.getYaw()));
mitchpang 0:58395e885409 359 pcMutex.unlock();
mitchpang 0:58395e885409 360 */
mitchpang 0:58395e885409 361 }
mitchpang 0:58395e885409 362
mitchpang 0:58395e885409 363 }