Ragnarok
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
main.cpp@2:e4ae1d748311, 2014-04-13 (annotated)
- Committer:
- JimCarver
- Date:
- Sun Apr 13 21:38:03 2014 +0000
- Revision:
- 2:e4ae1d748311
- Parent:
- 0:4e1d43dc608f
- Child:
- 3:0770c275e6e8
Improved version that uses the latest eCompass library and also supports the 6 axis FXOS8700 combo accelerometer and magnetometer (Requires shield with FXOS8700, I used the Freescale multi sensor shield)
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 | 2:e4ae1d748311 | 5 | #include "FXOS8700Q.h" |
JimCarver | 0:4e1d43dc608f | 6 | #include "rtos.h" |
JimCarver | 0:4e1d43dc608f | 7 | #include "SLCD.h" |
JimCarver | 0:4e1d43dc608f | 8 | |
JimCarver | 0:4e1d43dc608f | 9 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
JimCarver | 0:4e1d43dc608f | 10 | |
JimCarver | 0:4e1d43dc608f | 11 | |
JimCarver | 2:e4ae1d748311 | 12 | eCompass compass; |
JimCarver | 0:4e1d43dc608f | 13 | MAG3110 mag( PTE25, PTE24); |
JimCarver | 0:4e1d43dc608f | 14 | MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); |
JimCarver | 2:e4ae1d748311 | 15 | //FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0); |
JimCarver | 2:e4ae1d748311 | 16 | DigitalOut red(LED_RED); |
JimCarver | 2:e4ae1d748311 | 17 | DigitalOut green(LED_GREEN); |
JimCarver | 0:4e1d43dc608f | 18 | Serial pc(USBTX, USBRX); |
JimCarver | 0:4e1d43dc608f | 19 | SLCD slcd; |
JimCarver | 0:4e1d43dc608f | 20 | |
JimCarver | 2:e4ae1d748311 | 21 | extern axis6_t axis6; |
JimCarver | 2:e4ae1d748311 | 22 | extern uint32_t seconds; |
JimCarver | 2:e4ae1d748311 | 23 | extern uint32_t compass_type; |
JimCarver | 2:e4ae1d748311 | 24 | extern int32_t tcount; |
JimCarver | 2:e4ae1d748311 | 25 | extern uint8_t cdebug; |
JimCarver | 2:e4ae1d748311 | 26 | // |
JimCarver | 2:e4ae1d748311 | 27 | // Print data values for debug |
JimCarver | 2:e4ae1d748311 | 28 | // |
JimCarver | 2:e4ae1d748311 | 29 | void debug_print(void) |
JimCarver | 2:e4ae1d748311 | 30 | { |
JimCarver | 2:e4ae1d748311 | 31 | // Some useful printf statements for debug |
JimCarver | 2:e4ae1d748311 | 32 | printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); |
JimCarver | 2:e4ae1d748311 | 33 | printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); |
JimCarver | 2:e4ae1d748311 | 34 | printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); |
JimCarver | 2:e4ae1d748311 | 35 | printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\r\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); |
JimCarver | 2:e4ae1d748311 | 36 | } |
JimCarver | 2:e4ae1d748311 | 37 | // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors |
JimCarver | 2:e4ae1d748311 | 38 | // |
JimCarver | 0:4e1d43dc608f | 39 | // This routing move and negates data as needed the |
JimCarver | 0:4e1d43dc608f | 40 | // properly align the sensor axis for our desired compass (NED) |
JimCarver | 2:e4ae1d748311 | 41 | // For more information see Freescale appication note AN4696 |
JimCarver | 0:4e1d43dc608f | 42 | // |
JimCarver | 0:4e1d43dc608f | 43 | void hal_map(int16_t * acc, int16_t * mag) |
JimCarver | 0:4e1d43dc608f | 44 | { |
JimCarver | 0:4e1d43dc608f | 45 | int16_t t; |
JimCarver | 0:4e1d43dc608f | 46 | // swap accelerometer x & y |
JimCarver | 0:4e1d43dc608f | 47 | t = acc[1]; |
JimCarver | 0:4e1d43dc608f | 48 | acc[1] = acc[0]; |
JimCarver | 0:4e1d43dc608f | 49 | acc[0] = t; |
JimCarver | 0:4e1d43dc608f | 50 | // negate accelerometer x & y |
JimCarver | 0:4e1d43dc608f | 51 | acc[0] *= -1; |
JimCarver | 0:4e1d43dc608f | 52 | acc[1] *= -1; |
JimCarver | 0:4e1d43dc608f | 53 | // negate magnetometer y |
JimCarver | 0:4e1d43dc608f | 54 | mag[1] *= -1; |
JimCarver | 0:4e1d43dc608f | 55 | |
JimCarver | 0:4e1d43dc608f | 56 | } |
JimCarver | 0:4e1d43dc608f | 57 | |
JimCarver | 2:e4ae1d748311 | 58 | /* |
JimCarver | 2:e4ae1d748311 | 59 | // HAL Map for Multi-Sensor board with FXOS8700 sensor |
JimCarver | 2:e4ae1d748311 | 60 | // |
JimCarver | 2:e4ae1d748311 | 61 | // This routing move and negates data as needed the |
JimCarver | 2:e4ae1d748311 | 62 | // properly align the sensor axis for our desired compass (NED) |
JimCarver | 2:e4ae1d748311 | 63 | // For more information see Freescale appication note AN4696 |
JimCarver | 2:e4ae1d748311 | 64 | // |
JimCarver | 2:e4ae1d748311 | 65 | void hal_map( int16_t * acc, int16_t * mag) |
JimCarver | 0:4e1d43dc608f | 66 | { |
JimCarver | 2:e4ae1d748311 | 67 | int16_t t; |
JimCarver | 2:e4ae1d748311 | 68 | // Insert your HAL map here |
JimCarver | 2:e4ae1d748311 | 69 | t = acc[0]; |
JimCarver | 2:e4ae1d748311 | 70 | acc[0] = acc[1] * -1; |
JimCarver | 2:e4ae1d748311 | 71 | acc[1] = t * -1; |
JimCarver | 2:e4ae1d748311 | 72 | |
JimCarver | 2:e4ae1d748311 | 73 | t = mag[1]; |
JimCarver | 2:e4ae1d748311 | 74 | mag[1] = mag[0]; |
JimCarver | 2:e4ae1d748311 | 75 | mag[0] = t; |
JimCarver | 2:e4ae1d748311 | 76 | mag[2] *= -1; |
JimCarver | 2:e4ae1d748311 | 77 | |
JimCarver | 2:e4ae1d748311 | 78 | } |
JimCarver | 2:e4ae1d748311 | 79 | */ |
JimCarver | 2:e4ae1d748311 | 80 | |
JimCarver | 0:4e1d43dc608f | 81 | |
JimCarver | 0:4e1d43dc608f | 82 | // |
JimCarver | 0:4e1d43dc608f | 83 | // This is the 50Hz thread where the magic happens |
JimCarver | 0:4e1d43dc608f | 84 | // |
JimCarver | 2:e4ae1d748311 | 85 | int l = 0; |
JimCarver | 2:e4ae1d748311 | 86 | |
JimCarver | 0:4e1d43dc608f | 87 | void compass_thread(void const *argument) { |
JimCarver | 0:4e1d43dc608f | 88 | static int16_t acc_raw[3], mag_raw[3]; |
JimCarver | 0:4e1d43dc608f | 89 | // get raw data from the sensors |
JimCarver | 0:4e1d43dc608f | 90 | mag.ReadXYZraw(mag_raw); |
JimCarver | 0:4e1d43dc608f | 91 | acc.getAccXYZraw(acc_raw); |
JimCarver | 2:e4ae1d748311 | 92 | //combo0.AccXYZraw( acc_raw); |
JimCarver | 2:e4ae1d748311 | 93 | //combo0.MagXYZraw( mag_raw); |
JimCarver | 2:e4ae1d748311 | 94 | if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass |
JimCarver | 0:4e1d43dc608f | 95 | slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD |
JimCarver | 0:4e1d43dc608f | 96 | if(l++ >= 50) { // take car of business once a second |
JimCarver | 0:4e1d43dc608f | 97 | seconds++; |
JimCarver | 2:e4ae1d748311 | 98 | debug_print(); |
JimCarver | 2:e4ae1d748311 | 99 | compass.calibrate(); // re-calibrate the eCompass every second |
JimCarver | 0:4e1d43dc608f | 100 | l = 0; |
JimCarver | 2:e4ae1d748311 | 101 | green = !green; |
JimCarver | 0:4e1d43dc608f | 102 | } |
JimCarver | 0:4e1d43dc608f | 103 | tcount++; |
JimCarver | 0:4e1d43dc608f | 104 | } |
JimCarver | 0:4e1d43dc608f | 105 | |
JimCarver | 0:4e1d43dc608f | 106 | int main() { |
JimCarver | 2:e4ae1d748311 | 107 | // Thread thread(compass_thread); |
JimCarver | 2:e4ae1d748311 | 108 | int16_t mag_data[3]; |
JimCarver | 0:4e1d43dc608f | 109 | slcd.clear(); |
JimCarver | 0:4e1d43dc608f | 110 | tcount = 0; |
JimCarver | 2:e4ae1d748311 | 111 | red = 1; |
JimCarver | 2:e4ae1d748311 | 112 | green = 1; |
JimCarver | 2:e4ae1d748311 | 113 | //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. |
JimCarver | 0:4e1d43dc608f | 114 | compass_type = NED_COMPASS; |
JimCarver | 0:4e1d43dc608f | 115 | seconds = 0; |
JimCarver | 0:4e1d43dc608f | 116 | // make sure our sensors are talking to us |
JimCarver | 2:e4ae1d748311 | 117 | // Only available when multi-sensor shield is installed |
JimCarver | 2:e4ae1d748311 | 118 | //printf("\r\nFXOS8700 ID= %X\r\n", combo0.getWhoAmI()); |
JimCarver | 2:e4ae1d748311 | 119 | // NAtive KL46-FRDM sensors |
JimCarver | 2:e4ae1d748311 | 120 | printf("\r\nMMA8451Q ID= %X\r\n", acc.getWhoAmI()); |
JimCarver | 2:e4ae1d748311 | 121 | printf("MAG3110 ID= %X\r\n", mag.readReg(MAG_WHO_AM_I)); |
JimCarver | 2:e4ae1d748311 | 122 | mag.ReadXYZraw(mag_data); // flush the magnetmeter |
JimCarver | 2:e4ae1d748311 | 123 | RtosTimer compass_timer(compass_thread, osTimerPeriodic); |
JimCarver | 2:e4ae1d748311 | 124 | // //RtosTimer b_timer(blink_green, osTimerPeriodic); |
JimCarver | 2:e4ae1d748311 | 125 | compass_timer.start(20); // Run the Compass every 20ms |
JimCarver | 2:e4ae1d748311 | 126 | //b_timer.start(250); //blink the green LED at 2Hz |
JimCarver | 2:e4ae1d748311 | 127 | Thread::wait(osWaitForever); |
JimCarver | 0:4e1d43dc608f | 128 | } |