![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Final, cambios pendientes
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass_TiltCompensed_Acel-Mag by
Diff: main.cpp
- Revision:
- 3:0770c275e6e8
- Parent:
- 2:e4ae1d748311
- Child:
- 4:ba1dbfb683fb
diff -r e4ae1d748311 -r 0770c275e6e8 main.cpp --- a/main.cpp Sun Apr 13 21:38:03 2014 +0000 +++ b/main.cpp Fri May 16 18:23:32 2014 +0000 @@ -2,7 +2,6 @@ #include "eCompass_Lib.h" #include "MAG3110.h" #include "MMA8451Q.h" -#include "FXOS8700Q.h" #include "rtos.h" #include "SLCD.h" @@ -12,7 +11,6 @@ eCompass compass; MAG3110 mag( PTE25, PTE24); MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); -//FXOS8700Q combo0( A4, A5, FXOS8700CQ_SLAVE_ADDR0); DigitalOut red(LED_RED); DigitalOut green(LED_GREEN); Serial pc(USBTX, USBRX); @@ -23,6 +21,16 @@ extern uint32_t compass_type; extern int32_t tcount; extern uint8_t cdebug; + + +MotionSensorDataCounts mag_raw; +MotionSensorDataCounts acc_raw; + + +Thread *(debugp); +Thread *(calibrate); +Thread *(lcdp); + // // Print data values for debug // @@ -40,72 +48,68 @@ // 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) +void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data) { 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; + // swap and negate accelerometer x & y + t = acc_data->y; + acc_data->y = acc_data->x * -1; + acc_data->x = t * -1; + // negate magnetometer y - mag[1] *= -1; + mag_data->y *= -1; } -/* -// 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) -{ - 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]; // get raw data from the sensors - mag.ReadXYZraw(mag_raw); - acc.getAccXYZraw(acc_raw); - //combo0.AccXYZraw( acc_raw); - //combo0.MagXYZraw( mag_raw); + mag.getAxis(mag_raw); + acc.getAxis(acc_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 % 10)) lcdp->signal_set(0x04); if(l++ >= 50) { // take car of business once a second seconds++; - debug_print(); - compass.calibrate(); // re-calibrate the eCompass every second + calibrate->signal_set(0x2); + debugp->signal_set(0x01); l = 0; green = !green; } tcount++; } - + + +void calibrate_thread(void const *argument) { + while(true) { + Thread::signal_wait(0x2); + compass.calibrate(); + } +} + + +void print_thread(void const *argument) { + while (true) { + // Signal flags that are reported as event are automatically cleared. + Thread::signal_wait(0x1); + debug_print(); + } +} + + +void lcd_thread(void const *argument) { + while (true) { + // Signal flags that are reported as event are automatically cleared. + Thread::signal_wait(0x4); + slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD + } +} + int main() { -// Thread thread(compass_thread); - int16_t mag_data[3]; + slcd.clear(); tcount = 0; red = 1; @@ -113,16 +117,36 @@ //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data. compass_type = NED_COMPASS; seconds = 0; + Thread t1(print_thread); + Thread t2(calibrate_thread); + Thread t3(lcd_thread); + debugp = &t1; + calibrate = &t2; + lcdp = &t3; + mag.enable(); + acc.enable(); // make sure our sensors are talking to us // 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 + // Native KL46-FRDM sensors + printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI()); + printf("MAG3110 ID= %X\r\n", mag.whoAmI()); + mag.getAxis(mag_raw); // flush the magnetmeter RtosTimer compass_timer(compass_thread, osTimerPeriodic); -// //RtosTimer b_timer(blink_green, osTimerPeriodic); + + /* + * Thread Priorities + * Compass Thread, High Priority + * Compass calibration, Above Normal + * LED Update, Normal + * printf to console, Below Normal + * main(), Normal + */ + + debugp->set_priority(osPriorityBelowNormal); + lcdp->set_priority(osPriorityLow); + calibrate->set_priority(osPriorityAboveNormal); + compass_timer.start(20); // Run the Compass every 20ms - //b_timer.start(250); //blink the green LED at 2Hz Thread::wait(osWaitForever); }