This program uses the MMA8451 & MAG3110 on a KL-46 freedom board to implement a tilt compensated eCompass with the heading displayed on the LCD. This program uses the mbed RTOS to manage the hard real time requirements of the eCompass algorithm (Updated to mbed-os 5.x using eventqueues)
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib
Fork of KL46_eCompass by
main.cpp
- Committer:
- JimCarver
- Date:
- 2014-04-07
- Revision:
- 0:4e1d43dc608f
- Child:
- 2:e4ae1d748311
File content as of revision 0:4e1d43dc608f:
#include "mbed.h" #include "eCompass_Lib_V3.h" #include "MAG3110.h" #include "MMA8451Q.h" #include "rtos.h" #include "SLCD.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) MAG3110 mag( PTE25, PTE24); MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); I2C i2c( PTE25, PTE24); DigitalOut led(LED_RED); Serial pc(USBTX, USBRX); SLCD slcd; axis6_t axis6; unsigned int seconds; unsigned int compass_type; int tcount; // This routing move and negates data as needed the // properly align the sensor axis for our desired compass (NED) // void hal_map(int16_t * acc, int16_t * mag) { int16_t t; // swap accelerometer x & y t = acc[1]; acc[1] = acc[0]; acc[0] = t; // negate accelerometer x & y acc[0] *= -1; acc[1] *= -1; // negate magnetometer y mag[1] *= -1; } void manage_axis6(int16_t * acc_raw, int16_t * mag_raw) { axis6.timestamp = tcount; hal_map(acc_raw, mag_raw); // // raw data axis6.acc_x = acc_raw[0]; axis6.acc_y = acc_raw[1]; axis6.acc_z = acc_raw[2]; axis6.mag_x = mag_raw[0]; axis6.mag_y = mag_raw[1]; axis6.mag_z = mag_raw[2]; // // raw data converted to floating ouing axis6.fax = (float) acc_raw[0]; axis6.fay = (float) acc_raw[1]; axis6.faz = (float) acc_raw[2]; axis6.fmx = (float) mag_raw[0]; axis6.fmy = (float) mag_raw[1]; axis6.fmz = (float) mag_raw[2]; // // Accelerometer data converted to Gs axis6.fGax = ((float) acc_raw[0]) / 4096.0; axis6.fGay = ((float) acc_raw[1]) / 4096.0; axis6.fGaz = ((float) acc_raw[2]) / 4096.0; // // Magnetometer data converted to microteslas axis6.fUTmx = ((float) mag_raw[0]) / 10.0; axis6.fUTmy = ((float) mag_raw[1]) / 10.0; axis6.fUTmz = ((float) mag_raw[2]) / 10.0; } // // This is the 50Hz thread where the magic happens // void compass_thread(void const *argument) { static int16_t acc_raw[3], mag_raw[3]; static int l = 0; while (true) { // Signal flags that are reported as event are automatically cleared. Thread::signal_wait(0x1); // get raw data from the sensors mag.ReadXYZraw(mag_raw); acc.getAccXYZraw(acc_raw); manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass if(tcount) run_eCompass(); // calculate the eCompass slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD if(l++ >= 50) { // take car of business once a second seconds++; calibrate_eCompass(); // re-calibrate the eCompass every second l = 0; /* Some usful printf statements for debug printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); */ led = !led; } tcount++; } } int main() { Thread thread(compass_thread); float mag_data[3]; slcd.clear(); tcount = 0; init_eCompass(); compass_type = NED_COMPASS; seconds = 0; // make sure our sensors are talking to us printf("\r\n%X\r\n", acc.getWhoAmI()); printf("%X\r\n", mag.readReg(MAG_WHO_AM_I)); mag.ReadXYZ(mag_data); // flush the magnetmeter while (1) { Thread::wait(20); // run out thread every 20ms for a 50Hz rate thread.signal_set(0x1); } }