Unit testing and development for 9DOF sparkfun sensor stick
Dependencies: ADXL345 HMC5883L ITG3200 mbed
main.cpp@5:63b115f85aa7, 2012-11-09 (annotated)
- Committer:
- tylerjw
- Date:
- Fri Nov 09 18:33:29 2012 +0000
- Revision:
- 5:63b115f85aa7
- Parent:
- 4:8a77e21d08f1
ITG3200 curve fit test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tylerjw | 0:ac2f55940442 | 1 | /** |
tylerjw | 2:d7e66940541d | 2 | 9-dof unit testing |
tylerjw | 0:ac2f55940442 | 3 | |
tylerjw | 0:ac2f55940442 | 4 | The purpose of this program is to demonstrate and calibrate the three |
tylerjw | 0:ac2f55940442 | 5 | sensors on teh 9-doft board. |
tylerjw | 0:ac2f55940442 | 6 | |
tylerjw | 0:ac2f55940442 | 7 | The first version of this software will test the ITG3200 gyro to find |
tylerjw | 0:ac2f55940442 | 8 | the temperature drift line. Data will be logged to help determine the |
tylerjw | 0:ac2f55940442 | 9 | thermal drift line. |
tylerjw | 0:ac2f55940442 | 10 | See: http://mbed.org/users/gltest26/code/ITG3200/wiki/Thermal-Drift |
tylerjw | 1:dc730a26cdc2 | 11 | |
tylerjw | 1:dc730a26cdc2 | 12 | The second versoin of this will test the HMC5883L 3 axis magnometer |
tylerjw | 2:d7e66940541d | 13 | |
tylerjw | 2:d7e66940541d | 14 | The thrid version tests the ADXL345 library. |
tylerjw | 0:ac2f55940442 | 15 | */ |
tylerjw | 0:ac2f55940442 | 16 | |
tylerjw | 0:ac2f55940442 | 17 | #include "mbed.h" |
tylerjw | 4:8a77e21d08f1 | 18 | #include "ITG3200.h" |
tylerjw | 4:8a77e21d08f1 | 19 | #include "HMC5883L.h" |
tylerjw | 4:8a77e21d08f1 | 20 | #include "ADXL345.h" |
tylerjw | 4:8a77e21d08f1 | 21 | //#include "adxl345unit.h" |
tylerjw | 1:dc730a26cdc2 | 22 | |
tylerjw | 2:d7e66940541d | 23 | I2C i2c_bus(p28, p27); |
tylerjw | 3:5e21a352e236 | 24 | ADXL345 accel(i2c_bus); |
tylerjw | 4:8a77e21d08f1 | 25 | HMC5883L magno(i2c_bus); |
tylerjw | 4:8a77e21d08f1 | 26 | ITG3200 gyro(i2c_bus); |
tylerjw | 3:5e21a352e236 | 27 | Timer t; |
tylerjw | 3:5e21a352e236 | 28 | Serial pc(USBTX, USBRX); |
tylerjw | 3:5e21a352e236 | 29 | //ADXL345UNIT accel_test(i2c_bus, t); |
tylerjw | 0:ac2f55940442 | 30 | |
tylerjw | 4:8a77e21d08f1 | 31 | void sample_time_test(); |
tylerjw | 5:63b115f85aa7 | 32 | void itg3200_curve_test(); |
tylerjw | 4:8a77e21d08f1 | 33 | |
tylerjw | 0:ac2f55940442 | 34 | int main() |
tylerjw | 0:ac2f55940442 | 35 | { |
tylerjw | 5:63b115f85aa7 | 36 | itg3200_curve_test(); |
tylerjw | 4:8a77e21d08f1 | 37 | } |
tylerjw | 4:8a77e21d08f1 | 38 | |
tylerjw | 4:8a77e21d08f1 | 39 | void sample_time_test() |
tylerjw | 4:8a77e21d08f1 | 40 | { |
tylerjw | 4:8a77e21d08f1 | 41 | int16_t accel_sample[3]; |
tylerjw | 4:8a77e21d08f1 | 42 | int16_t magno_sample[3]; |
tylerjw | 4:8a77e21d08f1 | 43 | int16_t gyro_sample[3]; |
tylerjw | 4:8a77e21d08f1 | 44 | double temperature; |
tylerjw | 4:8a77e21d08f1 | 45 | double heading_rad, heading_deg; |
tylerjw | 4:8a77e21d08f1 | 46 | wait(0.5); // enough time for everything to prepare a sample |
tylerjw | 3:5e21a352e236 | 47 | t.start(); |
tylerjw | 4:8a77e21d08f1 | 48 | double accel_time, magno_time, gyro_time, temp_time, head_rad_time, head_deg_time; |
tylerjw | 3:5e21a352e236 | 49 | pc.baud(9600); |
tylerjw | 4:8a77e21d08f1 | 50 | |
tylerjw | 4:8a77e21d08f1 | 51 | pc.printf("accel,magno,gyro,temp,head_rad,head_deg\r\n"); |
tylerjw | 4:8a77e21d08f1 | 52 | for(int i = 0; i < 100; i++) |
tylerjw | 4:8a77e21d08f1 | 53 | { |
tylerjw | 4:8a77e21d08f1 | 54 | t.reset(); |
tylerjw | 4:8a77e21d08f1 | 55 | accel.getXYZ(accel_sample); |
tylerjw | 4:8a77e21d08f1 | 56 | accel_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 57 | magno.getXYZ(magno_sample); |
tylerjw | 4:8a77e21d08f1 | 58 | magno_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 59 | gyro.getXYZ(gyro_sample, ITG3200::Calibration); |
tylerjw | 4:8a77e21d08f1 | 60 | gyro_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 61 | temperature = gyro.getTemperature(); |
tylerjw | 4:8a77e21d08f1 | 62 | temp_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 63 | heading_rad = magno.getHeadingXY(); |
tylerjw | 4:8a77e21d08f1 | 64 | head_rad_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 65 | heading_deg = magno.getHeadingXYDeg(); |
tylerjw | 4:8a77e21d08f1 | 66 | head_deg_time = t.read(); |
tylerjw | 4:8a77e21d08f1 | 67 | |
tylerjw | 4:8a77e21d08f1 | 68 | pc.printf("%f,%f,%f,%f,%f,%f\r\n",accel_time,magno_time,gyro_time,temp_time,head_rad_time,head_deg_time); |
tylerjw | 4:8a77e21d08f1 | 69 | |
tylerjw | 4:8a77e21d08f1 | 70 | wait(0.2 - t.read()); |
tylerjw | 4:8a77e21d08f1 | 71 | } |
tylerjw | 4:8a77e21d08f1 | 72 | } |
tylerjw | 4:8a77e21d08f1 | 73 | |
tylerjw | 4:8a77e21d08f1 | 74 | void ADXL345_test() |
tylerjw | 4:8a77e21d08f1 | 75 | { |
tylerjw | 4:8a77e21d08f1 | 76 | int16_t sample[3]; |
tylerjw | 3:5e21a352e236 | 77 | float elapsed_time; |
tylerjw | 3:5e21a352e236 | 78 | float time_stamp[100]; |
tylerjw | 4:8a77e21d08f1 | 79 | int16_t samples[100][3]; |
tylerjw | 2:d7e66940541d | 80 | |
tylerjw | 3:5e21a352e236 | 81 | Timer t; |
tylerjw | 3:5e21a352e236 | 82 | Serial pc(USBTX, USBRX); |
tylerjw | 3:5e21a352e236 | 83 | pc.baud(9600); |
tylerjw | 3:5e21a352e236 | 84 | |
tylerjw | 3:5e21a352e236 | 85 | accel.setDataRate(ADXL345_100HZ); // 100Hz data rate |
tylerjw | 3:5e21a352e236 | 86 | accel.setDataFormatControl(ADXL345_16G | ADXL345_FULL_RES); |
tylerjw | 3:5e21a352e236 | 87 | accel.setPowerControl(0x08); // start measurement |
tylerjw | 3:5e21a352e236 | 88 | |
tylerjw | 3:5e21a352e236 | 89 | wait(0.001); |
tylerjw | 3:5e21a352e236 | 90 | |
tylerjw | 3:5e21a352e236 | 91 | t.start(); |
tylerjw | 4:8a77e21d08f1 | 92 | accel.getXYZ(sample); |
tylerjw | 3:5e21a352e236 | 93 | elapsed_time = t.read(); |
tylerjw | 3:5e21a352e236 | 94 | |
tylerjw | 3:5e21a352e236 | 95 | pc.printf("Sample took %f seconds.\r\n", elapsed_time); |
tylerjw | 3:5e21a352e236 | 96 | pc.printf("x: %d, y: %d, z: %d\r\n", sample[0],sample[1],sample[2]); |
tylerjw | 3:5e21a352e236 | 97 | |
tylerjw | 3:5e21a352e236 | 98 | for(int i = 0; i < 100; i++) { |
tylerjw | 3:5e21a352e236 | 99 | t.reset(); |
tylerjw | 4:8a77e21d08f1 | 100 | accel.getXYZ(samples[i]); |
tylerjw | 3:5e21a352e236 | 101 | time_stamp[i] = t.read(); |
tylerjw | 3:5e21a352e236 | 102 | } |
tylerjw | 3:5e21a352e236 | 103 | for(int i = 0; i < 100; i++) { |
tylerjw | 3:5e21a352e236 | 104 | pc.printf("Sample took %f seconds.\r\n", time_stamp[i]); |
tylerjw | 3:5e21a352e236 | 105 | pc.printf("x: %d, y: %d, z: %d\r\n", samples[i][0],samples[i][1],samples[i][2]); |
tylerjw | 3:5e21a352e236 | 106 | } |
tylerjw | 2:d7e66940541d | 107 | } |
tylerjw | 2:d7e66940541d | 108 | |
tylerjw | 2:d7e66940541d | 109 | void hmc5883l_test() |
tylerjw | 2:d7e66940541d | 110 | { |
tylerjw | 1:dc730a26cdc2 | 111 | Serial pc(USBTX, USBRX); |
tylerjw | 2:d7e66940541d | 112 | |
tylerjw | 1:dc730a26cdc2 | 113 | pc.baud(9600); |
tylerjw | 3:5e21a352e236 | 114 | |
tylerjw | 2:d7e66940541d | 115 | HMC5883L compass(i2c_bus); |
tylerjw | 2:d7e66940541d | 116 | |
tylerjw | 4:8a77e21d08f1 | 117 | int16_t data[3]; |
tylerjw | 2:d7e66940541d | 118 | |
tylerjw | 2:d7e66940541d | 119 | while(1) { |
tylerjw | 2:d7e66940541d | 120 | compass.getXYZ(data); |
tylerjw | 2:d7e66940541d | 121 | pc.printf("x: %4d, y: %4d, z: %4d\r\n", data[0], data[1], data[2]); |
tylerjw | 1:dc730a26cdc2 | 122 | wait(0.067); |
tylerjw | 1:dc730a26cdc2 | 123 | } |
tylerjw | 1:dc730a26cdc2 | 124 | } |
tylerjw | 4:8a77e21d08f1 | 125 | |
tylerjw | 4:8a77e21d08f1 | 126 | void itg3200_curve_test() |
tylerjw | 1:dc730a26cdc2 | 127 | { |
tylerjw | 1:dc730a26cdc2 | 128 | DigitalOut myled(LED1); |
tylerjw | 0:ac2f55940442 | 129 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
tylerjw | 0:ac2f55940442 | 130 | ITG3200 gyro(p28, p27); // sda, scl - gyro |
tylerjw | 5:63b115f85aa7 | 131 | |
tylerjw | 5:63b115f85aa7 | 132 | /* |
tylerjw | 1:dc730a26cdc2 | 133 | const float offset[3] = {99.5, -45.0, -29.7}; // taken from itg3200.xls curve fit test |
tylerjw | 1:dc730a26cdc2 | 134 | const float slope[3] = {-1.05, 0.95, 0.47}; |
tylerjw | 1:dc730a26cdc2 | 135 | |
tylerjw | 1:dc730a26cdc2 | 136 | gyro.setCalibrationCurve(offset, slope); |
tylerjw | 5:63b115f85aa7 | 137 | gyro.setLpBandwidth(LPFBW_42HZ); |
tylerjw | 5:63b115f85aa7 | 138 | */ |
tylerjw | 1:dc730a26cdc2 | 139 | |
tylerjw | 0:ac2f55940442 | 140 | Serial pc(USBTX, USBRX); |
tylerjw | 0:ac2f55940442 | 141 | |
tylerjw | 0:ac2f55940442 | 142 | pc.baud(9600); |
tylerjw | 0:ac2f55940442 | 143 | |
tylerjw | 0:ac2f55940442 | 144 | myled = 0; |
tylerjw | 0:ac2f55940442 | 145 | FILE *fp = fopen("/local/itg3200.csv", "w"); // Open "itg3200.csv" for writing |
tylerjw | 0:ac2f55940442 | 146 | fputs("Temp, X, Y, Z\r\n", fp); // place the header at the top |
tylerjw | 0:ac2f55940442 | 147 | |
tylerjw | 0:ac2f55940442 | 148 | float temperature = 0.0; |
tylerjw | 4:8a77e21d08f1 | 149 | int16_t gyro_readings[3]; |
tylerjw | 0:ac2f55940442 | 150 | |
tylerjw | 0:ac2f55940442 | 151 | for(int i = 0; i < 120; i++) { // 120 seconds - 600 samples |
tylerjw | 0:ac2f55940442 | 152 | myled = 1; |
tylerjw | 5:63b115f85aa7 | 153 | gyro.calibrate(1.0); |
tylerjw | 0:ac2f55940442 | 154 | myled = 0; |
tylerjw | 5:63b115f85aa7 | 155 | //gyro.getXYZ(gyro_readings, ITG3200::Calibration); |
tylerjw | 5:63b115f85aa7 | 156 | gyro.getOffset(gyro_readings); |
tylerjw | 0:ac2f55940442 | 157 | temperature = gyro.getTemperature(); |
tylerjw | 0:ac2f55940442 | 158 | pc.printf("%3d,%f,%d,%d,%d\r\n",i,temperature,gyro_readings[0],gyro_readings[1],gyro_readings[2]); |
tylerjw | 0:ac2f55940442 | 159 | fprintf(fp, "%f,%d,%d,%d\r\n",temperature,gyro_readings[0],gyro_readings[1],gyro_readings[2]); |
tylerjw | 0:ac2f55940442 | 160 | } |
tylerjw | 0:ac2f55940442 | 161 | fclose(fp); |
tylerjw | 0:ac2f55940442 | 162 | myled = 0; |
tylerjw | 0:ac2f55940442 | 163 | } |