default eCompass example, but with desired serial communication
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
main.cpp@7:d932835d9613, 2016-04-08 (annotated)
- Committer:
- Kosinkadink
- Date:
- Fri Apr 08 04:43:29 2016 +0000
- Revision:
- 7:d932835d9613
- Parent:
- 6:f68052364f92
good stuff m9
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JimCarver | 0:4e1d43dc608f | 1 | #include "mbed.h" |
JimCarver | 2:e4ae1d748311 | 2 | #include "eCompass_Lib.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 | 2:e4ae1d748311 | 11 | eCompass compass; |
JimCarver | 0:4e1d43dc608f | 12 | MAG3110 mag( PTE25, PTE24); |
JimCarver | 0:4e1d43dc608f | 13 | MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); |
JimCarver | 2:e4ae1d748311 | 14 | DigitalOut red(LED_RED); |
JimCarver | 2:e4ae1d748311 | 15 | DigitalOut green(LED_GREEN); |
JimCarver | 0:4e1d43dc608f | 16 | Serial pc(USBTX, USBRX); |
JimCarver | 0:4e1d43dc608f | 17 | SLCD slcd; |
JimCarver | 0:4e1d43dc608f | 18 | |
JimCarver | 2:e4ae1d748311 | 19 | extern axis6_t axis6; |
JimCarver | 2:e4ae1d748311 | 20 | extern uint32_t seconds; |
JimCarver | 2:e4ae1d748311 | 21 | extern uint32_t compass_type; |
JimCarver | 2:e4ae1d748311 | 22 | extern int32_t tcount; |
JimCarver | 2:e4ae1d748311 | 23 | extern uint8_t cdebug; |
JimCarver | 3:0770c275e6e8 | 24 | |
JimCarver | 3:0770c275e6e8 | 25 | |
JimCarver | 3:0770c275e6e8 | 26 | MotionSensorDataCounts mag_raw; |
JimCarver | 3:0770c275e6e8 | 27 | MotionSensorDataCounts acc_raw; |
JimCarver | 3:0770c275e6e8 | 28 | |
JimCarver | 3:0770c275e6e8 | 29 | |
JimCarver | 3:0770c275e6e8 | 30 | Thread *(debugp); |
JimCarver | 3:0770c275e6e8 | 31 | Thread *(calibrate); |
JimCarver | 3:0770c275e6e8 | 32 | Thread *(lcdp); |
JimCarver | 3:0770c275e6e8 | 33 | |
JimCarver | 2:e4ae1d748311 | 34 | // |
JimCarver | 2:e4ae1d748311 | 35 | // Print data values for debug |
JimCarver | 2:e4ae1d748311 | 36 | // |
JimCarver | 2:e4ae1d748311 | 37 | void debug_print(void) |
JimCarver | 2:e4ae1d748311 | 38 | { |
Kosinkadink | 7:d932835d9613 | 39 | //pc.printf("0,%d$",axis6.yaw); |
Kosinkadink | 7:d932835d9613 | 40 | pc.printf("%4.2f,%4.2f$",axis6.fUTmx, axis6.fUTmy); |
Kosinkadink | 5:25d98baa1ded | 41 | //int h, m, s; |
Kosinkadink | 5:25d98baa1ded | 42 | //h = seconds / 3600; |
Kosinkadink | 5:25d98baa1ded | 43 | //m = (seconds % 3600) / 60; |
Kosinkadink | 5:25d98baa1ded | 44 | //s = seconds % 60; |
JimCarver | 2:e4ae1d748311 | 45 | // Some useful printf statements for debug |
Kosinkadink | 5:25d98baa1ded | 46 | //printf("Runtime= %d:%02d:%02d\r\n", h, m, s); |
Kosinkadink | 5:25d98baa1ded | 47 | //printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); |
Kosinkadink | 5:25d98baa1ded | 48 | //printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); |
Kosinkadink | 5:25d98baa1ded | 49 | //printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); |
Kosinkadink | 5:25d98baa1ded | 50 | //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); |
JimCarver | 2:e4ae1d748311 | 51 | } |
JimCarver | 2:e4ae1d748311 | 52 | // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors |
JimCarver | 2:e4ae1d748311 | 53 | // |
JimCarver | 0:4e1d43dc608f | 54 | // This routing move and negates data as needed the |
JimCarver | 0:4e1d43dc608f | 55 | // properly align the sensor axis for our desired compass (NED) |
JimCarver | 2:e4ae1d748311 | 56 | // For more information see Freescale appication note AN4696 |
JimCarver | 0:4e1d43dc608f | 57 | // |
JimCarver | 3:0770c275e6e8 | 58 | void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data) |
JimCarver | 0:4e1d43dc608f | 59 | { |
Kosinkadink | 7:d932835d9613 | 60 | //int16_t t; |
JimCarver | 3:0770c275e6e8 | 61 | // swap and negate accelerometer x & y |
Kosinkadink | 7:d932835d9613 | 62 | //t = acc_data->y; |
Kosinkadink | 7:d932835d9613 | 63 | //acc_data->y = acc_data->x * -1; |
Kosinkadink | 7:d932835d9613 | 64 | //acc_data->x = t * -1; |
JimCarver | 3:0770c275e6e8 | 65 | |
JimCarver | 0:4e1d43dc608f | 66 | // negate magnetometer y |
Kosinkadink | 7:d932835d9613 | 67 | //mag_data->y *= -1; |
JimCarver | 0:4e1d43dc608f | 68 | |
JimCarver | 0:4e1d43dc608f | 69 | } |
JimCarver | 0:4e1d43dc608f | 70 | |
JimCarver | 0:4e1d43dc608f | 71 | // |
JimCarver | 0:4e1d43dc608f | 72 | // This is the 50Hz thread where the magic happens |
JimCarver | 0:4e1d43dc608f | 73 | // |
JimCarver | 2:e4ae1d748311 | 74 | int l = 0; |
JimCarver | 2:e4ae1d748311 | 75 | |
JimCarver | 0:4e1d43dc608f | 76 | void compass_thread(void const *argument) { |
JimCarver | 0:4e1d43dc608f | 77 | // get raw data from the sensors |
JimCarver | 3:0770c275e6e8 | 78 | mag.getAxis(mag_raw); |
JimCarver | 3:0770c275e6e8 | 79 | acc.getAxis(acc_raw); |
JimCarver | 2:e4ae1d748311 | 80 | if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass |
JimCarver | 3:0770c275e6e8 | 81 | if(!(l % 10)) lcdp->signal_set(0x04); |
JimCarver | 0:4e1d43dc608f | 82 | if(l++ >= 50) { // take car of business once a second |
JimCarver | 0:4e1d43dc608f | 83 | seconds++; |
Kosinkadink | 7:d932835d9613 | 84 | //calibrate->signal_set(0x2); |
Kosinkadink | 6:f68052364f92 | 85 | //debugp->signal_set(0x01); |
JimCarver | 0:4e1d43dc608f | 86 | l = 0; |
JimCarver | 2:e4ae1d748311 | 87 | green = !green; |
JimCarver | 0:4e1d43dc608f | 88 | } |
JimCarver | 0:4e1d43dc608f | 89 | tcount++; |
JimCarver | 0:4e1d43dc608f | 90 | } |
JimCarver | 3:0770c275e6e8 | 91 | |
JimCarver | 3:0770c275e6e8 | 92 | |
JimCarver | 3:0770c275e6e8 | 93 | void calibrate_thread(void const *argument) { |
JimCarver | 3:0770c275e6e8 | 94 | while(true) { |
JimCarver | 3:0770c275e6e8 | 95 | Thread::signal_wait(0x2); |
Kosinkadink | 7:d932835d9613 | 96 | //red = 0; |
Kosinkadink | 7:d932835d9613 | 97 | //compass.calibrate(); |
Kosinkadink | 7:d932835d9613 | 98 | //red = 1; |
JimCarver | 3:0770c275e6e8 | 99 | } |
JimCarver | 3:0770c275e6e8 | 100 | } |
JimCarver | 3:0770c275e6e8 | 101 | |
JimCarver | 3:0770c275e6e8 | 102 | |
JimCarver | 3:0770c275e6e8 | 103 | void print_thread(void const *argument) { |
JimCarver | 3:0770c275e6e8 | 104 | while (true) { |
JimCarver | 3:0770c275e6e8 | 105 | // Signal flags that are reported as event are automatically cleared. |
Kosinkadink | 5:25d98baa1ded | 106 | //Thread::signal_wait(0x1); |
Kosinkadink | 5:25d98baa1ded | 107 | if (pc.getc()) |
Kosinkadink | 5:25d98baa1ded | 108 | { |
Kosinkadink | 5:25d98baa1ded | 109 | debug_print(); |
Kosinkadink | 5:25d98baa1ded | 110 | } |
JimCarver | 3:0770c275e6e8 | 111 | } |
JimCarver | 3:0770c275e6e8 | 112 | } |
JimCarver | 3:0770c275e6e8 | 113 | |
JimCarver | 3:0770c275e6e8 | 114 | |
JimCarver | 3:0770c275e6e8 | 115 | void lcd_thread(void const *argument) { |
JimCarver | 3:0770c275e6e8 | 116 | while (true) { |
JimCarver | 3:0770c275e6e8 | 117 | // Signal flags that are reported as event are automatically cleared. |
Kosinkadink | 6:f68052364f92 | 118 | Thread::signal_wait(0x04); |
Kosinkadink | 6:f68052364f92 | 119 | slcd.printf("%04d", axis6.yaw); |
JimCarver | 3:0770c275e6e8 | 120 | } |
JimCarver | 3:0770c275e6e8 | 121 | } |
JimCarver | 3:0770c275e6e8 | 122 | |
JimCarver | 0:4e1d43dc608f | 123 | int main() { |
JimCarver | 3:0770c275e6e8 | 124 | |
JimCarver | 0:4e1d43dc608f | 125 | slcd.clear(); |
JimCarver | 0:4e1d43dc608f | 126 | tcount = 0; |
JimCarver | 2:e4ae1d748311 | 127 | red = 1; |
JimCarver | 2:e4ae1d748311 | 128 | green = 1; |
JimCarver | 2:e4ae1d748311 | 129 | //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. |
JimCarver | 0:4e1d43dc608f | 130 | compass_type = NED_COMPASS; |
JimCarver | 0:4e1d43dc608f | 131 | seconds = 0; |
JimCarver | 3:0770c275e6e8 | 132 | Thread t1(print_thread); |
JimCarver | 3:0770c275e6e8 | 133 | Thread t2(calibrate_thread); |
JimCarver | 3:0770c275e6e8 | 134 | Thread t3(lcd_thread); |
JimCarver | 3:0770c275e6e8 | 135 | debugp = &t1; |
JimCarver | 3:0770c275e6e8 | 136 | calibrate = &t2; |
JimCarver | 3:0770c275e6e8 | 137 | lcdp = &t3; |
JimCarver | 3:0770c275e6e8 | 138 | mag.enable(); |
JimCarver | 3:0770c275e6e8 | 139 | acc.enable(); |
JimCarver | 4:ba1dbfb683fb | 140 | // Say hello to our sensors |
JimCarver | 3:0770c275e6e8 | 141 | // Native KL46-FRDM sensors |
Kosinkadink | 5:25d98baa1ded | 142 | //printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI()); |
Kosinkadink | 5:25d98baa1ded | 143 | //printf("MAG3110 ID= %X\r\n", mag.whoAmI()); |
JimCarver | 3:0770c275e6e8 | 144 | mag.getAxis(mag_raw); // flush the magnetmeter |
JimCarver | 2:e4ae1d748311 | 145 | RtosTimer compass_timer(compass_thread, osTimerPeriodic); |
JimCarver | 3:0770c275e6e8 | 146 | |
JimCarver | 3:0770c275e6e8 | 147 | /* |
JimCarver | 3:0770c275e6e8 | 148 | * Thread Priorities |
JimCarver | 3:0770c275e6e8 | 149 | * Compass Thread, High Priority |
JimCarver | 3:0770c275e6e8 | 150 | * Compass calibration, Above Normal |
JimCarver | 3:0770c275e6e8 | 151 | * LED Update, Normal |
JimCarver | 3:0770c275e6e8 | 152 | * printf to console, Below Normal |
JimCarver | 3:0770c275e6e8 | 153 | * main(), Normal |
JimCarver | 3:0770c275e6e8 | 154 | */ |
JimCarver | 3:0770c275e6e8 | 155 | |
JimCarver | 3:0770c275e6e8 | 156 | debugp->set_priority(osPriorityBelowNormal); |
JimCarver | 3:0770c275e6e8 | 157 | lcdp->set_priority(osPriorityLow); |
JimCarver | 3:0770c275e6e8 | 158 | calibrate->set_priority(osPriorityAboveNormal); |
JimCarver | 2:e4ae1d748311 | 159 | compass_timer.start(20); // Run the Compass every 20ms |
JimCarver | 4:ba1dbfb683fb | 160 | |
JimCarver | 2:e4ae1d748311 | 161 | Thread::wait(osWaitForever); |
JimCarver | 0:4e1d43dc608f | 162 | } |