Jun1x / Mbed OS KL46_eCompass

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 Jim Carver

main.cpp@6:95bced85b8e5, 2018-02-05 (annotated)

Committer:
jun1x
Date:
Mon Feb 05 16:26:22 2018 +0000
Revision:
6:95bced85b8e5
Parent:
5:7b95c2f8e76e 
Change mbed 2.x threads and rtostimer to mbed 5.x eventqueue

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimCarver 0:4e1d43dc608f 1 #include "mbed.h"
jun1x 6:95bced85b8e5 2 #include "mbed_events.h"
JimCarver 2:e4ae1d748311 3 #include "eCompass_Lib.h"
JimCarver 0:4e1d43dc608f 4 #include "MAG3110.h"
JimCarver 0:4e1d43dc608f 5 #include "MMA8451Q.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 2:e4ae1d748311 10 eCompass compass;
JimCarver 0:4e1d43dc608f 11 MAG3110 mag( PTE25, PTE24);
JimCarver 0:4e1d43dc608f 12 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
JimCarver 2:e4ae1d748311 13 DigitalOut red(LED_RED);
JimCarver 2:e4ae1d748311 14 DigitalOut green(LED_GREEN);
JimCarver 0:4e1d43dc608f 15 Serial pc(USBTX, USBRX);
JimCarver 0:4e1d43dc608f 16 SLCD slcd;
JimCarver 0:4e1d43dc608f 17
JimCarver 2:e4ae1d748311 18 extern axis6_t axis6;
JimCarver 2:e4ae1d748311 19 extern uint32_t seconds;
JimCarver 2:e4ae1d748311 20 extern uint32_t compass_type;
JimCarver 2:e4ae1d748311 21 extern int32_t tcount;
JimCarver 2:e4ae1d748311 22 extern uint8_t cdebug;
JimCarver 3:0770c275e6e8 23
JimCarver 3:0770c275e6e8 24
JimCarver 3:0770c275e6e8 25 MotionSensorDataCounts mag_raw;
JimCarver 3:0770c275e6e8 26 MotionSensorDataCounts acc_raw;
JimCarver 3:0770c275e6e8 27
jun1x 6:95bced85b8e5 28 /*
jun1x 6:95bced85b8e5 29 * Thread Priorities
jun1x 6:95bced85b8e5 30 * Compass Thread, High Priority
jun1x 6:95bced85b8e5 31 * Compass calibration, Above Normal
jun1x 6:95bced85b8e5 32 * Console/LCD Update, Below Normal
jun1x 6:95bced85b8e5 33 * main(), Normal
jun1x 6:95bced85b8e5 34 */
jun1x 6:95bced85b8e5 35
jun1x 6:95bced85b8e5 36 Thread threadCalibrate(osPriorityAboveNormal);
jun1x 6:95bced85b8e5 37 Thread threadCompass(osPriorityHigh);
jun1x 6:95bced85b8e5 38 Thread threadDebug(osPriorityBelowNormal);
JimCarver 3:0770c275e6e8 39
jun1x 6:95bced85b8e5 40 EventQueue queueCalibrate;
jun1x 6:95bced85b8e5 41 EventQueue queueCompass;
jun1x 6:95bced85b8e5 42 EventQueue queueDebug;
jun1x 6:95bced85b8e5 43
JimCarver 2:e4ae1d748311 44 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
JimCarver 2:e4ae1d748311 45 //
JimCarver 0:4e1d43dc608f 46 // This routing move and negates data as needed the
JimCarver 0:4e1d43dc608f 47 // properly align the sensor axis for our desired compass (NED)
JimCarver 2:e4ae1d748311 48 // For more information see Freescale appication note AN4696
JimCarver 0:4e1d43dc608f 49 //
JimCarver 3:0770c275e6e8 50 void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
JimCarver 0:4e1d43dc608f 51 {
JimCarver 0:4e1d43dc608f 52 int16_t t;
JimCarver 3:0770c275e6e8 53 // swap and negate accelerometer x & y
JimCarver 3:0770c275e6e8 54 t = acc_data->y;
JimCarver 3:0770c275e6e8 55 acc_data->y = acc_data->x * -1;
JimCarver 3:0770c275e6e8 56 acc_data->x = t * -1;
JimCarver 3:0770c275e6e8 57
JimCarver 0:4e1d43dc608f 58 // negate magnetometer y
JimCarver 3:0770c275e6e8 59 mag_data->y *= -1;
JimCarver 0:4e1d43dc608f 60 }
JimCarver 0:4e1d43dc608f 61
JimCarver 0:4e1d43dc608f 62 //
JimCarver 0:4e1d43dc608f 63 // This is the 50Hz thread where the magic happens
JimCarver 0:4e1d43dc608f 64 //
JimCarver 2:e4ae1d748311 65 int l = 0;
jun1x 6:95bced85b8e5 66 void compass_thread() {
jun1x 6:95bced85b8e5 67 // get raw data from the sensors
jun1x 6:95bced85b8e5 68 mag.getAxis(mag_raw);
jun1x 6:95bced85b8e5 69 acc.getAxis(acc_raw);
jun1x 6:95bced85b8e5 70 if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
jun1x 6:95bced85b8e5 71 if(l++ >= 50) { // take car of business once a second
jun1x 6:95bced85b8e5 72 seconds++;
jun1x 6:95bced85b8e5 73 l = 0;
jun1x 6:95bced85b8e5 74 green = !green;
jun1x 6:95bced85b8e5 75 }
jun1x 6:95bced85b8e5 76 tcount++;
JimCarver 3:0770c275e6e8 77 }
JimCarver 3:0770c275e6e8 78
jun1x 6:95bced85b8e5 79 //
jun1x 6:95bced85b8e5 80 // Recalibrate compass at 1Hz
jun1x 6:95bced85b8e5 81 //
jun1x 6:95bced85b8e5 82 void calibrate_thread() {
jun1x 6:95bced85b8e5 83 red = !red;
jun1x 6:95bced85b8e5 84 compass.calibrate();
jun1x 6:95bced85b8e5 85 }
JimCarver 3:0770c275e6e8 86
jun1x 6:95bced85b8e5 87 //
jun1x 6:95bced85b8e5 88 // Print data values for debug at 1Hz
jun1x 6:95bced85b8e5 89 //
jun1x 6:95bced85b8e5 90 void debug_thread(void)
jun1x 6:95bced85b8e5 91 {
jun1x 6:95bced85b8e5 92 int h, m, s;
jun1x 6:95bced85b8e5 93 h = seconds / 3600;
jun1x 6:95bced85b8e5 94 m = (seconds % 3600) / 60;
jun1x 6:95bced85b8e5 95 s = seconds % 60;
jun1x 6:95bced85b8e5 96 // Some useful printf statements for debug
jun1x 6:95bced85b8e5 97 printf("Runtime= %d:%02d:%02d\r\n", h, m, s);
jun1x 6:95bced85b8e5 98 printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
jun1x 6:95bced85b8e5 99 printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz);
jun1x 6:95bced85b8e5 100 printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
jun1x 6:95bced85b8e5 101 printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3);
jun1x 6:95bced85b8e5 102 }
JimCarver 3:0770c275e6e8 103
jun1x 6:95bced85b8e5 104 //
jun1x 6:95bced85b8e5 105 // Update LCD at 5Hz
jun1x 6:95bced85b8e5 106 //
jun1x 6:95bced85b8e5 107 void lcd_thread() {
jun1x 6:95bced85b8e5 108 slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
JimCarver 3:0770c275e6e8 109 }
jun1x 6:95bced85b8e5 110
JimCarver 0:4e1d43dc608f 111 int main() {
JimCarver 0:4e1d43dc608f 112 slcd.clear();
JimCarver 2:e4ae1d748311 113 red = 1;
JimCarver 2:e4ae1d748311 114 green = 1;
jun1x 6:95bced85b8e5 115
jun1x 6:95bced85b8e5 116 seconds = 0;
jun1x 6:95bced85b8e5 117 compass_type = NED_COMPASS;
jun1x 6:95bced85b8e5 118 tcount = 0;
jun1x 6:95bced85b8e5 119 cdebug = 0; // Set to 1 to disable eCompass in order to observe raw mag data.
jun1x 6:95bced85b8e5 120
JimCarver 3:0770c275e6e8 121 mag.enable();
JimCarver 3:0770c275e6e8 122 acc.enable();
JimCarver 4:ba1dbfb683fb 123 // Say hello to our sensors
JimCarver 3:0770c275e6e8 124 // Native KL46-FRDM sensors
JimCarver 3:0770c275e6e8 125 printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI());
JimCarver 3:0770c275e6e8 126 printf("MAG3110 ID= %X\r\n", mag.whoAmI());
JimCarver 3:0770c275e6e8 127 mag.getAxis(mag_raw); // flush the magnetmeter
JimCarver 3:0770c275e6e8 128
jun1x 6:95bced85b8e5 129 // Events on the queues are called periodically, interval in milliseconds
jun1x 6:95bced85b8e5 130 queueCalibrate.call_every(1000, calibrate_thread);
jun1x 6:95bced85b8e5 131 queueCompass.call_every(20, compass_thread);
jun1x 6:95bced85b8e5 132 queueDebug.call_every(200, lcd_thread);
jun1x 6:95bced85b8e5 133 queueDebug.call_every(1000, debug_thread);
JimCarver 4:ba1dbfb683fb 134
jun1x 6:95bced85b8e5 135 // Queues run in different threads with different priorities
jun1x 6:95bced85b8e5 136 threadCalibrate.start(callback(&queueCalibrate, &EventQueue::dispatch_forever));
jun1x 6:95bced85b8e5 137 threadCompass.start(callback(&queueCompass, &EventQueue::dispatch_forever));
jun1x 6:95bced85b8e5 138 threadDebug.start(callback(&queueDebug, &EventQueue::dispatch_forever));
JimCarver 0:4e1d43dc608f 139 }