Laura Delaney / Mbed 2 deprecated MINILAB_DAY10

Use a touchscreen display to select which sensor measurement to display

Dependencies:   mbed DisplayModule24_demo_day10

compass.cpp@8:a7b7edb66de5, 2017-01-17 (annotated)

Committer:
ldelaney17
Date:
Tue Jan 17 21:28:13 2017 +0000
Revision:
8:a7b7edb66de5
Parent:
7:b275c76de4ba 
final commit; cleaned up debug code; added comments; set 10s ticker period

Who changed what in which revision?

UserRevisionLine numberNew 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 8:a7b7edb66de5 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 8:a7b7edb66de5 50 //gets the average of n readings
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 }