Irving Hernandez / Mbed 2 deprecated Ragnarok_2ejes

Final, cambios pendientes

Dependencies:   MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed

Fork of KL46_eCompass_TiltCompensed_Acel-Mag by Irving Hernandez

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?

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