// Kalman filter sensor fusion task
void Fusion_task(uint32_t task_init_data)
{
uint16_t LedGreenCounter = 0;
// initialize the sensor fusion algorithms
Fusion_Init();
// initialize the user medium frequency (typically 25Hz) task
UserMediumFrequencyTaskInit();
// infinite loop controlled by MQX-Lite events
while(1)
{
// ensure the red LED (power up check) is off (line high)
LED_RED_SetVal(NULL);
// set the output test pin to zero (for timing measurements)
TestPin_KF_Time_ClrVal(NULL);
// wait for the sensor fusion event to occur
// FALSE means any bit (of the 1 bit enabled by the mask) unblocks
// and NULL means timeout is infinite
_lwevent_wait_for(&(mqxglobals.RunKFEventStruct), 1, FALSE, NULL);
// set the output test pin to high to denote sensor fusion running
TestPin_KF_Time_SetVal(NULL);
// flash the green LED to denote the sensor fusion is running
if (++LedGreenCounter >= 5)
{
LED_GREEN_NegVal(NULL);
LedGreenCounter = 0;
}
// reset the magnetic calibration flag (this is set by the fusion algorithm)
mqxglobals.MagCal_Event_Flag = 0;
// call the sensor fusion algorithms
Fusion_Run();
// run the user medium frequency (typically 25Hz) user task
UserMediumFrequencyTaskRun();
// enable the magnetic calibration event if the flag for a new magnetic calibration
// with a mask of 1 (least significant bit set)
// was set by the sensor fusion algorithms
if (mqxglobals.MagCal_Event_Flag)
{
_lwevent_set(&(mqxglobals.MagCalEventStruct), 1);
}
} // end of infinite loop
return;
}
/*lint -save -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
/* Write your local variable definition here */
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
/*** End of Processor Expert internal initialization. ***/
LED_GREEN_NegVal();
LED_RED_NegVal();
LED_YELLOW_NegVal();
LED_ORANGE_NegVal();
MySegLCDPtr = SegLCD1_Init(NULL); //initialize sLCD according to PEx
vfnLCD_Write_Msg("1234");
/*Turn on all special symbols*/
_FREESCALE_ON();
_ONE_ON();
_COL_ON();
_GRADE_ON();
_PERCENTAGE_ON();
_AM_ON();
_PM_ON();
for (main_counter=1000000 ; main_counter>0 ;main_counter--){}; //delay to show all symbol ON
/*Turn off all special symbols*/
_FREESCALE_OFF();
_ONE_OFF();
_COL_OFF();
_GRADE_OFF();
_PERCENTAGE_OFF();
_AM_OFF();
_PM_OFF();
#ifdef CW
// If we are using CodeWarrior, we must call this function to change
// the buffer behavior such that a new line character, "\n", is not
// required to print characters to the terminal.
setvbuf(stdout, NULL, _IONBF, 0);
#endif
printf("\n\r KL46 sLCD test \n");
printf("\n\r press a=all symbols on, 0=all symbols Off, 1 to 6 symbols");
_FREESCALE_ON();
for(;;)
{
/*echoes all received character to terminal */
InpData[0] = getchar();
printf("%c",InpData[0]);
main_counter++;
sprintf(sLCDBuffer,"%04i",main_counter%1000);
vfnLCD_Write_Msg(sLCDBuffer);
LED_GREEN_NegVal();
LED_RED_NegVal();
LED_YELLOW_NegVal();
LED_ORANGE_NegVal();
switch ( InpData[0])
{
case 'a':
_FREESCALE_ON();
_ONE_ON();
_COL_ON();
_GRADE_ON();
_PERCENTAGE_ON();
_AM_ON();
_PM_ON();
break;
case '0':
_FREESCALE_OFF();
_ONE_OFF();
_COL_OFF();
_GRADE_OFF();
_PERCENTAGE_OFF();
_AM_OFF();
_PM_OFF();
break;
case '1':
_FREESCALE_ON();
break;
case '2':
_COL_ON();
break;
case '3':
_GRADE_ON();
break;
case '4':
_PERCENTAGE_ON();
break;
case '5':
_AM_ON();
break;
case '6':
_PM_ON();
break;
default:
_FREESCALE_ON();
_ONE_OFF();
_COL_OFF();
_GRADE_OFF();
_PERCENTAGE_OFF();
_AM_OFF();
_PM_OFF();
}
}
/*** Don't write any code pass this line, or it will be deleted during code generation. ***/
/*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
#ifdef PEX_RTOS_START
PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/*** End of RTOS startup code. ***/
/*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
for(;;){}
/*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/