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@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?
User | Revision | Line number | New 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 | } |