Use a touchscreen display to select which sensor measurement to display
Dependencies: mbed DisplayModule24_demo_day10
compass.cpp@5:8a2afc6fdeb0, 2017-01-17 (annotated)
- Committer:
- ldelaney17
- Date:
- Tue Jan 17 19:23:00 2017 +0000
- Revision:
- 5:8a2afc6fdeb0
- Child:
- 7:b275c76de4ba
add compass.h and compass.cpp
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ldelaney17 | 5:8a2afc6fdeb0 | 1 | #include "mbed.h" |
ldelaney17 | 5:8a2afc6fdeb0 | 2 | #include "compass.h" |
ldelaney17 | 5:8a2afc6fdeb0 | 3 | |
ldelaney17 | 5:8a2afc6fdeb0 | 4 | I2C *compass; |
ldelaney17 | 5:8a2afc6fdeb0 | 5 | const int compass_addr = 0x1E << 1; |
ldelaney17 | 5:8a2afc6fdeb0 | 6 | int compass_avg_num = 1; |
ldelaney17 | 5:8a2afc6fdeb0 | 7 | const char config_single_measure_mode[2] = {0x02, 0x01}; |
ldelaney17 | 5:8a2afc6fdeb0 | 8 | const char compass_config_data[1] = {0x03}; |
ldelaney17 | 5:8a2afc6fdeb0 | 9 | |
ldelaney17 | 5:8a2afc6fdeb0 | 10 | void compass_init(){ |
ldelaney17 | 5:8a2afc6fdeb0 | 11 | compass = &connection; |
ldelaney17 | 5:8a2afc6fdeb0 | 12 | compass->write(compass_addr, config_single_measure_mode, 2); //set single measure mode |
ldelaney17 | 5:8a2afc6fdeb0 | 13 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 14 | |
ldelaney17 | 5:8a2afc6fdeb0 | 15 | |
ldelaney17 | 5:8a2afc6fdeb0 | 16 | float get_heading(float declination){ |
ldelaney17 | 5:8a2afc6fdeb0 | 17 | char rawBytes[6]; |
ldelaney17 | 5:8a2afc6fdeb0 | 18 | short data[6]; |
ldelaney17 | 5:8a2afc6fdeb0 | 19 | short magComponents[3]; //x, z, y |
ldelaney17 | 5:8a2afc6fdeb0 | 20 | |
ldelaney17 | 5:8a2afc6fdeb0 | 21 | compass->write(compass_addr, config_single_measure_mode, 2); //set single measure mode |
ldelaney17 | 5:8a2afc6fdeb0 | 22 | compass->write(compass_addr, compass_config_data, 1); //choose where to read from |
ldelaney17 | 5:8a2afc6fdeb0 | 23 | compass->read(compass_addr, rawBytes, 6); //read in 6 bytes |
ldelaney17 | 5:8a2afc6fdeb0 | 24 | |
ldelaney17 | 5:8a2afc6fdeb0 | 25 | //process the data |
ldelaney17 | 5:8a2afc6fdeb0 | 26 | //move to data |
ldelaney17 | 5:8a2afc6fdeb0 | 27 | for (int i = 0; i < 6; i++){ |
ldelaney17 | 5:8a2afc6fdeb0 | 28 | data[i] = rawBytes[i]; |
ldelaney17 | 5:8a2afc6fdeb0 | 29 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 30 | //do bitshift things |
ldelaney17 | 5:8a2afc6fdeb0 | 31 | //move to magcomps (x, z, y) |
ldelaney17 | 5:8a2afc6fdeb0 | 32 | for (int i = 0; i < 3; i++){ |
ldelaney17 | 5:8a2afc6fdeb0 | 33 | magComponents[i] = data[2*i] << 8 | data[2*i + 1]; |
ldelaney17 | 5:8a2afc6fdeb0 | 34 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 35 | return calc_heading(magComponents, declination); |
ldelaney17 | 5:8a2afc6fdeb0 | 36 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 37 | |
ldelaney17 | 5:8a2afc6fdeb0 | 38 | float calc_heading(short mags[3], float declination){ |
ldelaney17 | 5:8a2afc6fdeb0 | 39 | double heading; |
ldelaney17 | 5:8a2afc6fdeb0 | 40 | heading = atan2((double)mags[2],(double)mags[0]); |
ldelaney17 | 5:8a2afc6fdeb0 | 41 | heading = heading * 180 / 3.141593; //convert to radians |
ldelaney17 | 5:8a2afc6fdeb0 | 42 | heading += declination; //adjust for mag north vs true north |
ldelaney17 | 5:8a2afc6fdeb0 | 43 | if ( heading > 180) //keep the heading between -180 and 180 degrees |
ldelaney17 | 5:8a2afc6fdeb0 | 44 | heading -=360; |
ldelaney17 | 5:8a2afc6fdeb0 | 45 | #ifdef DEBUG_MODE |
ldelaney17 | 5:8a2afc6fdeb0 | 46 | pc.printf("Heading: %f\r\n", heading); |
ldelaney17 | 5:8a2afc6fdeb0 | 47 | #endif |
ldelaney17 | 5:8a2afc6fdeb0 | 48 | return heading; |
ldelaney17 | 5:8a2afc6fdeb0 | 49 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 50 | |
ldelaney17 | 5:8a2afc6fdeb0 | 51 | float get_avg_heading(float declination, int n){ |
ldelaney17 | 5:8a2afc6fdeb0 | 52 | if (n <= 0) |
ldelaney17 | 5:8a2afc6fdeb0 | 53 | return 0; |
ldelaney17 | 5:8a2afc6fdeb0 | 54 | double total_heading = 0; |
ldelaney17 | 5:8a2afc6fdeb0 | 55 | for (int i = 0; i < n; i++){ |
ldelaney17 | 5:8a2afc6fdeb0 | 56 | total_heading += get_heading(declination); //sum up the total of n readings |
ldelaney17 | 5:8a2afc6fdeb0 | 57 | } |
ldelaney17 | 5:8a2afc6fdeb0 | 58 | return total_heading / n; //divide by n to get the average |
ldelaney17 | 5:8a2afc6fdeb0 | 59 | } |