Jed Kos / Mbed 2 deprecated compass_acc

default eCompass example, but with desired serial communication

Dependencies:   MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed

Fork of KL46_eCompass by Jim Carver

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?

UserRevisionLine numberNew 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 }