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@0:4e1d43dc608f, 2014-04-07 (annotated)

Committer:
JimCarver
Date:
Mon Apr 07 21:04:11 2014 +0000
Revision:
0:4e1d43dc608f
Child:
2:e4ae1d748311
Minor change to make less dependent on Wi-Go

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimCarver 0:4e1d43dc608f 1 #include "mbed.h"
JimCarver 0:4e1d43dc608f 2 #include "eCompass_Lib_V3.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 0:4e1d43dc608f 11 MAG3110 mag( PTE25, PTE24);
JimCarver 0:4e1d43dc608f 12 MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
JimCarver 0:4e1d43dc608f 13 I2C i2c( PTE25, PTE24);
JimCarver 0:4e1d43dc608f 14 DigitalOut led(LED_RED);
JimCarver 0:4e1d43dc608f 15 Serial pc(USBTX, USBRX);
JimCarver 0:4e1d43dc608f 16 SLCD slcd;
JimCarver 0:4e1d43dc608f 17
JimCarver 0:4e1d43dc608f 18 axis6_t axis6;
JimCarver 0:4e1d43dc608f 19 unsigned int seconds;
JimCarver 0:4e1d43dc608f 20 unsigned int compass_type;
JimCarver 0:4e1d43dc608f 21 int tcount;
JimCarver 0:4e1d43dc608f 22
JimCarver 0:4e1d43dc608f 23 // This routing move and negates data as needed the
JimCarver 0:4e1d43dc608f 24 // properly align the sensor axis for our desired compass (NED)
JimCarver 0:4e1d43dc608f 25 //
JimCarver 0:4e1d43dc608f 26 void hal_map(int16_t * acc, int16_t * mag)
JimCarver 0:4e1d43dc608f 27 {
JimCarver 0:4e1d43dc608f 28 int16_t t;
JimCarver 0:4e1d43dc608f 29 // swap accelerometer x & y
JimCarver 0:4e1d43dc608f 30 t = acc[1];
JimCarver 0:4e1d43dc608f 31 acc[1] = acc[0];
JimCarver 0:4e1d43dc608f 32 acc[0] = t;
JimCarver 0:4e1d43dc608f 33 // negate accelerometer x & y
JimCarver 0:4e1d43dc608f 34 acc[0] *= -1;
JimCarver 0:4e1d43dc608f 35 acc[1] *= -1;
JimCarver 0:4e1d43dc608f 36 // negate magnetometer y
JimCarver 0:4e1d43dc608f 37 mag[1] *= -1;
JimCarver 0:4e1d43dc608f 38
JimCarver 0:4e1d43dc608f 39 }
JimCarver 0:4e1d43dc608f 40
JimCarver 0:4e1d43dc608f 41
JimCarver 0:4e1d43dc608f 42 void manage_axis6(int16_t * acc_raw, int16_t * mag_raw)
JimCarver 0:4e1d43dc608f 43 {
JimCarver 0:4e1d43dc608f 44 axis6.timestamp = tcount;
JimCarver 0:4e1d43dc608f 45 hal_map(acc_raw, mag_raw);
JimCarver 0:4e1d43dc608f 46 //
JimCarver 0:4e1d43dc608f 47 // raw data
JimCarver 0:4e1d43dc608f 48 axis6.acc_x = acc_raw[0];
JimCarver 0:4e1d43dc608f 49 axis6.acc_y = acc_raw[1];
JimCarver 0:4e1d43dc608f 50 axis6.acc_z = acc_raw[2];
JimCarver 0:4e1d43dc608f 51 axis6.mag_x = mag_raw[0];
JimCarver 0:4e1d43dc608f 52 axis6.mag_y = mag_raw[1];
JimCarver 0:4e1d43dc608f 53 axis6.mag_z = mag_raw[2];
JimCarver 0:4e1d43dc608f 54 //
JimCarver 0:4e1d43dc608f 55 // raw data converted to floating ouing
JimCarver 0:4e1d43dc608f 56 axis6.fax = (float) acc_raw[0];
JimCarver 0:4e1d43dc608f 57 axis6.fay = (float) acc_raw[1];
JimCarver 0:4e1d43dc608f 58 axis6.faz = (float) acc_raw[2];
JimCarver 0:4e1d43dc608f 59 axis6.fmx = (float) mag_raw[0];
JimCarver 0:4e1d43dc608f 60 axis6.fmy = (float) mag_raw[1];
JimCarver 0:4e1d43dc608f 61 axis6.fmz = (float) mag_raw[2];
JimCarver 0:4e1d43dc608f 62 //
JimCarver 0:4e1d43dc608f 63 // Accelerometer data converted to Gs
JimCarver 0:4e1d43dc608f 64 axis6.fGax = ((float) acc_raw[0]) / 4096.0;
JimCarver 0:4e1d43dc608f 65 axis6.fGay = ((float) acc_raw[1]) / 4096.0;
JimCarver 0:4e1d43dc608f 66 axis6.fGaz = ((float) acc_raw[2]) / 4096.0;
JimCarver 0:4e1d43dc608f 67 //
JimCarver 0:4e1d43dc608f 68 // Magnetometer data converted to microteslas
JimCarver 0:4e1d43dc608f 69 axis6.fUTmx = ((float) mag_raw[0]) / 10.0;
JimCarver 0:4e1d43dc608f 70 axis6.fUTmy = ((float) mag_raw[1]) / 10.0;
JimCarver 0:4e1d43dc608f 71 axis6.fUTmz = ((float) mag_raw[2]) / 10.0;
JimCarver 0:4e1d43dc608f 72 }
JimCarver 0:4e1d43dc608f 73
JimCarver 0:4e1d43dc608f 74 //
JimCarver 0:4e1d43dc608f 75 // This is the 50Hz thread where the magic happens
JimCarver 0:4e1d43dc608f 76 //
JimCarver 0:4e1d43dc608f 77 void compass_thread(void const *argument) {
JimCarver 0:4e1d43dc608f 78 static int16_t acc_raw[3], mag_raw[3];
JimCarver 0:4e1d43dc608f 79 static int l = 0;
JimCarver 0:4e1d43dc608f 80 while (true) {
JimCarver 0:4e1d43dc608f 81 // Signal flags that are reported as event are automatically cleared.
JimCarver 0:4e1d43dc608f 82 Thread::signal_wait(0x1);
JimCarver 0:4e1d43dc608f 83 // get raw data from the sensors
JimCarver 0:4e1d43dc608f 84 mag.ReadXYZraw(mag_raw);
JimCarver 0:4e1d43dc608f 85 acc.getAccXYZraw(acc_raw);
JimCarver 0:4e1d43dc608f 86
JimCarver 0:4e1d43dc608f 87 manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass
JimCarver 0:4e1d43dc608f 88 if(tcount) run_eCompass(); // calculate the eCompass
JimCarver 0:4e1d43dc608f 89 slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
JimCarver 0:4e1d43dc608f 90 if(l++ >= 50) { // take car of business once a second
JimCarver 0:4e1d43dc608f 91 seconds++;
JimCarver 0:4e1d43dc608f 92 calibrate_eCompass(); // re-calibrate the eCompass every second
JimCarver 0:4e1d43dc608f 93 l = 0;
JimCarver 0:4e1d43dc608f 94 /* Some usful printf statements for debug
JimCarver 0:4e1d43dc608f 95 printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
JimCarver 0:4e1d43dc608f 96 printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz);
JimCarver 0:4e1d43dc608f 97 printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
JimCarver 0:4e1d43dc608f 98 */
JimCarver 0:4e1d43dc608f 99 led = !led;
JimCarver 0:4e1d43dc608f 100 }
JimCarver 0:4e1d43dc608f 101 tcount++;
JimCarver 0:4e1d43dc608f 102 }
JimCarver 0:4e1d43dc608f 103 }
JimCarver 0:4e1d43dc608f 104
JimCarver 0:4e1d43dc608f 105 int main() {
JimCarver 0:4e1d43dc608f 106 Thread thread(compass_thread);
JimCarver 0:4e1d43dc608f 107 float mag_data[3];
JimCarver 0:4e1d43dc608f 108
JimCarver 0:4e1d43dc608f 109 slcd.clear();
JimCarver 0:4e1d43dc608f 110 tcount = 0;
JimCarver 0:4e1d43dc608f 111
JimCarver 0:4e1d43dc608f 112 init_eCompass();
JimCarver 0:4e1d43dc608f 113 compass_type = NED_COMPASS;
JimCarver 0:4e1d43dc608f 114 seconds = 0;
JimCarver 0:4e1d43dc608f 115 // make sure our sensors are talking to us
JimCarver 0:4e1d43dc608f 116 printf("\r\n%X\r\n", acc.getWhoAmI());
JimCarver 0:4e1d43dc608f 117 printf("%X\r\n", mag.readReg(MAG_WHO_AM_I));
JimCarver 0:4e1d43dc608f 118 mag.ReadXYZ(mag_data); // flush the magnetmeter
JimCarver 0:4e1d43dc608f 119 while (1) {
JimCarver 0:4e1d43dc608f 120 Thread::wait(20); // run out thread every 20ms for a 50Hz rate
JimCarver 0:4e1d43dc608f 121 thread.signal_set(0x1);
JimCarver 0:4e1d43dc608f 122 }
JimCarver 0:4e1d43dc608f 123 }