This program uses the MMA8451 & MAG3110 on a KL-46 freedom board to implement a tilt compensated eCompass with the heading displayed on the LCD. This program uses the mbed RTOS to manage the hard real time requirements of the eCompass algorithm (Updated to mbed-os 5.x using eventqueues)
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib
Fork of KL46_eCompass by
main.cpp@0:4e1d43dc608f, 2014-04-07 (annotated)
- Committer:
- JimCarver
- Date:
- Mon Apr 07 21:04:11 2014 +0000
- Revision:
- 0:4e1d43dc608f
- Child:
- 2:e4ae1d748311
Minor change to make less dependent on Wi-Go
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JimCarver | 0:4e1d43dc608f | 1 | #include "mbed.h" |
JimCarver | 0:4e1d43dc608f | 2 | #include "eCompass_Lib_V3.h" |
JimCarver | 0:4e1d43dc608f | 3 | #include "MAG3110.h" |
JimCarver | 0:4e1d43dc608f | 4 | #include "MMA8451Q.h" |
JimCarver | 0:4e1d43dc608f | 5 | #include "rtos.h" |
JimCarver | 0:4e1d43dc608f | 6 | #include "SLCD.h" |
JimCarver | 0:4e1d43dc608f | 7 | |
JimCarver | 0:4e1d43dc608f | 8 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
JimCarver | 0:4e1d43dc608f | 9 | |
JimCarver | 0:4e1d43dc608f | 10 | |
JimCarver | 0:4e1d43dc608f | 11 | MAG3110 mag( PTE25, PTE24); |
JimCarver | 0:4e1d43dc608f | 12 | MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); |
JimCarver | 0:4e1d43dc608f | 13 | I2C i2c( PTE25, PTE24); |
JimCarver | 0:4e1d43dc608f | 14 | DigitalOut led(LED_RED); |
JimCarver | 0:4e1d43dc608f | 15 | Serial pc(USBTX, USBRX); |
JimCarver | 0:4e1d43dc608f | 16 | SLCD slcd; |
JimCarver | 0:4e1d43dc608f | 17 | |
JimCarver | 0:4e1d43dc608f | 18 | axis6_t axis6; |
JimCarver | 0:4e1d43dc608f | 19 | unsigned int seconds; |
JimCarver | 0:4e1d43dc608f | 20 | unsigned int compass_type; |
JimCarver | 0:4e1d43dc608f | 21 | int tcount; |
JimCarver | 0:4e1d43dc608f | 22 | |
JimCarver | 0:4e1d43dc608f | 23 | // This routing move and negates data as needed the |
JimCarver | 0:4e1d43dc608f | 24 | // properly align the sensor axis for our desired compass (NED) |
JimCarver | 0:4e1d43dc608f | 25 | // |
JimCarver | 0:4e1d43dc608f | 26 | void hal_map(int16_t * acc, int16_t * mag) |
JimCarver | 0:4e1d43dc608f | 27 | { |
JimCarver | 0:4e1d43dc608f | 28 | int16_t t; |
JimCarver | 0:4e1d43dc608f | 29 | // swap accelerometer x & y |
JimCarver | 0:4e1d43dc608f | 30 | t = acc[1]; |
JimCarver | 0:4e1d43dc608f | 31 | acc[1] = acc[0]; |
JimCarver | 0:4e1d43dc608f | 32 | acc[0] = t; |
JimCarver | 0:4e1d43dc608f | 33 | // negate accelerometer x & y |
JimCarver | 0:4e1d43dc608f | 34 | acc[0] *= -1; |
JimCarver | 0:4e1d43dc608f | 35 | acc[1] *= -1; |
JimCarver | 0:4e1d43dc608f | 36 | // negate magnetometer y |
JimCarver | 0:4e1d43dc608f | 37 | mag[1] *= -1; |
JimCarver | 0:4e1d43dc608f | 38 | |
JimCarver | 0:4e1d43dc608f | 39 | } |
JimCarver | 0:4e1d43dc608f | 40 | |
JimCarver | 0:4e1d43dc608f | 41 | |
JimCarver | 0:4e1d43dc608f | 42 | void manage_axis6(int16_t * acc_raw, int16_t * mag_raw) |
JimCarver | 0:4e1d43dc608f | 43 | { |
JimCarver | 0:4e1d43dc608f | 44 | axis6.timestamp = tcount; |
JimCarver | 0:4e1d43dc608f | 45 | hal_map(acc_raw, mag_raw); |
JimCarver | 0:4e1d43dc608f | 46 | // |
JimCarver | 0:4e1d43dc608f | 47 | // raw data |
JimCarver | 0:4e1d43dc608f | 48 | axis6.acc_x = acc_raw[0]; |
JimCarver | 0:4e1d43dc608f | 49 | axis6.acc_y = acc_raw[1]; |
JimCarver | 0:4e1d43dc608f | 50 | axis6.acc_z = acc_raw[2]; |
JimCarver | 0:4e1d43dc608f | 51 | axis6.mag_x = mag_raw[0]; |
JimCarver | 0:4e1d43dc608f | 52 | axis6.mag_y = mag_raw[1]; |
JimCarver | 0:4e1d43dc608f | 53 | axis6.mag_z = mag_raw[2]; |
JimCarver | 0:4e1d43dc608f | 54 | // |
JimCarver | 0:4e1d43dc608f | 55 | // raw data converted to floating ouing |
JimCarver | 0:4e1d43dc608f | 56 | axis6.fax = (float) acc_raw[0]; |
JimCarver | 0:4e1d43dc608f | 57 | axis6.fay = (float) acc_raw[1]; |
JimCarver | 0:4e1d43dc608f | 58 | axis6.faz = (float) acc_raw[2]; |
JimCarver | 0:4e1d43dc608f | 59 | axis6.fmx = (float) mag_raw[0]; |
JimCarver | 0:4e1d43dc608f | 60 | axis6.fmy = (float) mag_raw[1]; |
JimCarver | 0:4e1d43dc608f | 61 | axis6.fmz = (float) mag_raw[2]; |
JimCarver | 0:4e1d43dc608f | 62 | // |
JimCarver | 0:4e1d43dc608f | 63 | // Accelerometer data converted to Gs |
JimCarver | 0:4e1d43dc608f | 64 | axis6.fGax = ((float) acc_raw[0]) / 4096.0; |
JimCarver | 0:4e1d43dc608f | 65 | axis6.fGay = ((float) acc_raw[1]) / 4096.0; |
JimCarver | 0:4e1d43dc608f | 66 | axis6.fGaz = ((float) acc_raw[2]) / 4096.0; |
JimCarver | 0:4e1d43dc608f | 67 | // |
JimCarver | 0:4e1d43dc608f | 68 | // Magnetometer data converted to microteslas |
JimCarver | 0:4e1d43dc608f | 69 | axis6.fUTmx = ((float) mag_raw[0]) / 10.0; |
JimCarver | 0:4e1d43dc608f | 70 | axis6.fUTmy = ((float) mag_raw[1]) / 10.0; |
JimCarver | 0:4e1d43dc608f | 71 | axis6.fUTmz = ((float) mag_raw[2]) / 10.0; |
JimCarver | 0:4e1d43dc608f | 72 | } |
JimCarver | 0:4e1d43dc608f | 73 | |
JimCarver | 0:4e1d43dc608f | 74 | // |
JimCarver | 0:4e1d43dc608f | 75 | // This is the 50Hz thread where the magic happens |
JimCarver | 0:4e1d43dc608f | 76 | // |
JimCarver | 0:4e1d43dc608f | 77 | void compass_thread(void const *argument) { |
JimCarver | 0:4e1d43dc608f | 78 | static int16_t acc_raw[3], mag_raw[3]; |
JimCarver | 0:4e1d43dc608f | 79 | static int l = 0; |
JimCarver | 0:4e1d43dc608f | 80 | while (true) { |
JimCarver | 0:4e1d43dc608f | 81 | // Signal flags that are reported as event are automatically cleared. |
JimCarver | 0:4e1d43dc608f | 82 | Thread::signal_wait(0x1); |
JimCarver | 0:4e1d43dc608f | 83 | // get raw data from the sensors |
JimCarver | 0:4e1d43dc608f | 84 | mag.ReadXYZraw(mag_raw); |
JimCarver | 0:4e1d43dc608f | 85 | acc.getAccXYZraw(acc_raw); |
JimCarver | 0:4e1d43dc608f | 86 | |
JimCarver | 0:4e1d43dc608f | 87 | manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass |
JimCarver | 0:4e1d43dc608f | 88 | if(tcount) run_eCompass(); // calculate the eCompass |
JimCarver | 0:4e1d43dc608f | 89 | slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD |
JimCarver | 0:4e1d43dc608f | 90 | if(l++ >= 50) { // take car of business once a second |
JimCarver | 0:4e1d43dc608f | 91 | seconds++; |
JimCarver | 0:4e1d43dc608f | 92 | calibrate_eCompass(); // re-calibrate the eCompass every second |
JimCarver | 0:4e1d43dc608f | 93 | l = 0; |
JimCarver | 0:4e1d43dc608f | 94 | /* Some usful printf statements for debug |
JimCarver | 0:4e1d43dc608f | 95 | printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); |
JimCarver | 0:4e1d43dc608f | 96 | printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); |
JimCarver | 0:4e1d43dc608f | 97 | printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); |
JimCarver | 0:4e1d43dc608f | 98 | */ |
JimCarver | 0:4e1d43dc608f | 99 | led = !led; |
JimCarver | 0:4e1d43dc608f | 100 | } |
JimCarver | 0:4e1d43dc608f | 101 | tcount++; |
JimCarver | 0:4e1d43dc608f | 102 | } |
JimCarver | 0:4e1d43dc608f | 103 | } |
JimCarver | 0:4e1d43dc608f | 104 | |
JimCarver | 0:4e1d43dc608f | 105 | int main() { |
JimCarver | 0:4e1d43dc608f | 106 | Thread thread(compass_thread); |
JimCarver | 0:4e1d43dc608f | 107 | float mag_data[3]; |
JimCarver | 0:4e1d43dc608f | 108 | |
JimCarver | 0:4e1d43dc608f | 109 | slcd.clear(); |
JimCarver | 0:4e1d43dc608f | 110 | tcount = 0; |
JimCarver | 0:4e1d43dc608f | 111 | |
JimCarver | 0:4e1d43dc608f | 112 | init_eCompass(); |
JimCarver | 0:4e1d43dc608f | 113 | compass_type = NED_COMPASS; |
JimCarver | 0:4e1d43dc608f | 114 | seconds = 0; |
JimCarver | 0:4e1d43dc608f | 115 | // make sure our sensors are talking to us |
JimCarver | 0:4e1d43dc608f | 116 | printf("\r\n%X\r\n", acc.getWhoAmI()); |
JimCarver | 0:4e1d43dc608f | 117 | printf("%X\r\n", mag.readReg(MAG_WHO_AM_I)); |
JimCarver | 0:4e1d43dc608f | 118 | mag.ReadXYZ(mag_data); // flush the magnetmeter |
JimCarver | 0:4e1d43dc608f | 119 | while (1) { |
JimCarver | 0:4e1d43dc608f | 120 | Thread::wait(20); // run out thread every 20ms for a 50Hz rate |
JimCarver | 0:4e1d43dc608f | 121 | thread.signal_set(0x1); |
JimCarver | 0:4e1d43dc608f | 122 | } |
JimCarver | 0:4e1d43dc608f | 123 | } |