![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Ragnarok
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
Diff: main.cpp
- Revision:
- 2:e4ae1d748311
- Parent:
- 0:4e1d43dc608f
- Child:
- 3:0770c275e6e8
--- a/main.cpp Mon Apr 07 21:07:04 2014 +0000 +++ b/main.cpp Sun Apr 13 21:38:03 2014 +0000 @@ -1,27 +1,44 @@ #include "mbed.h" -#include "eCompass_Lib_V3.h" +#include "eCompass_Lib.h" #include "MAG3110.h" #include "MMA8451Q.h" +#include "FXOS8700Q.h" #include "rtos.h" #include "SLCD.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) +eCompass compass; MAG3110 mag( PTE25, PTE24); MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); -I2C i2c( PTE25, PTE24); -DigitalOut led(LED_RED); +//FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0); +DigitalOut red(LED_RED); +DigitalOut green(LED_GREEN); Serial pc(USBTX, USBRX); SLCD slcd; -axis6_t axis6; -unsigned int seconds; -unsigned int compass_type; -int tcount; - +extern axis6_t axis6; +extern uint32_t seconds; +extern uint32_t compass_type; +extern int32_t tcount; +extern uint8_t cdebug; +// +// Print data values for debug +// +void debug_print(void) +{ + // Some useful 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", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); + 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); +} +// HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors +// // This routing move and negates data as needed the // properly align the sensor axis for our desired compass (NED) +// For more information see Freescale appication note AN4696 // void hal_map(int16_t * acc, int16_t * mag) { @@ -37,87 +54,75 @@ mag[1] *= -1; } - -void manage_axis6(int16_t * acc_raw, int16_t * mag_raw) +/* +// HAL Map for Multi-Sensor board with FXOS8700 sensor +// +// This routing move and negates data as needed the +// properly align the sensor axis for our desired compass (NED) +// For more information see Freescale appication note AN4696 +// +void hal_map( int16_t * acc, int16_t * mag) { - 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; - } + int16_t t; + // Insert your HAL map here + t = acc[0]; + acc[0] = acc[1] * -1; + acc[1] = t * -1; + + t = mag[1]; + mag[1] = mag[0]; + mag[0] = t; + mag[2] *= -1; + +} +*/ + // // This is the 50Hz thread where the magic happens // +int l = 0; + 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 + //combo0.AccXYZraw( acc_raw); + //combo0.MagXYZraw( mag_raw); + if(tcount) compass.run( acc_raw, mag_raw); // 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 + debug_print(); + compass.calibrate(); // 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; + green = !green; } tcount++; - } } int main() { - Thread thread(compass_thread); - float mag_data[3]; - +// Thread thread(compass_thread); + int16_t mag_data[3]; slcd.clear(); tcount = 0; - - init_eCompass(); + red = 1; + green = 1; + //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. 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); - } + // Only available when multi-sensor shield is installed + //printf("\r\nFXOS8700 ID= %X\r\n", combo0.getWhoAmI()); + // NAtive KL46-FRDM sensors + printf("\r\nMMA8451Q ID= %X\r\n", acc.getWhoAmI()); + printf("MAG3110 ID= %X\r\n", mag.readReg(MAG_WHO_AM_I)); + mag.ReadXYZraw(mag_data); // flush the magnetmeter + RtosTimer compass_timer(compass_thread, osTimerPeriodic); +// //RtosTimer b_timer(blink_green, osTimerPeriodic); + compass_timer.start(20); // Run the Compass every 20ms + //b_timer.start(250); //blink the green LED at 2Hz + Thread::wait(osWaitForever); }