/**
* This task will send any response data out of the UART.
*
* This should be called in the UART_OnTxChar event. Internally, the method checks to make sure the buffer
* is empty and that the buffer is ready to send, so this method can also be called in the main application
* loop.
*
* This method will automagically wiggle the TXEN line when necessary.
*/
void Command_ResponseTask()
{
if(ResponseReadyToSend == FALSE)
return;
// if(UART_PDD_GetTxCompleteStatus(UART1_BASE_PTR) != 0x40u) // we're waiting for a transmit to complete already
// return;
if(UART_GetCharsInTxBuf() > 0)
return; // we're waiting for a transmit to complete already
if(ResponseIndex == 0)
{
TXEN_SetVal();
for(int i=0;i<10000;i++);
}
// We have to wait a bit before we send our first byte.
UART_SendChar(ResponseBuffer[ResponseIndex++]);
if(ResponseIndex == COMMAND_RESPONSE_BUFFER_SIZE) // we've sent everything
{
// while(!(UART_PDD_ReadStatus1Flags(UART1_BASE_PTR) && UART_S1_TC_MASK) )
while(UART_PDD_GetTxCompleteStatus(UART1_BASE_PTR) == 0U)
// while(UART_GetCharsInTxBuf() > 0 );
// for(int i=0;i<10000;i++);
ResponseIndex = 0;
ResponseReadyToSend = FALSE;
TXEN_ClrVal();
LED_GREEN_SetVal();
}
}
// Main task
void Main_task(uint32_t task_init_data)
{
// switch the red LED on (line low sets LED on)
LED_RED_ClrVal(NULL);
LED_GREEN_SetVal(NULL);
LED_BLUE_SetVal(NULL);
// create the sensor sampling event (typically 200Hz)
_lwevent_create(&(mqxglobals.SamplingEventStruct), LWEVENT_AUTO_CLEAR);
// create the Kalman filter sensor fusion event (typically 25Hz)
_lwevent_create(&(mqxglobals.RunKFEventStruct), LWEVENT_AUTO_CLEAR);
// create the magnetic calibration event (typically once per minute)
_lwevent_create(&(mqxglobals.MagCalEventStruct), LWEVENT_AUTO_CLEAR);
// create the sensor read task (controlled by sensor sampling event SamplingEventStruct)
_task_create_at(0, RDSENSDATA_TASK, 0, RdSensData_task_stack, RDSENSDATA_TASK_STACK_SIZE);
// create the sensor fusion task (controlled by sensor fusion event RunKFEventStruct)
_task_create_at(0, FUSION_TASK, 0, Fusion_task_stack, FUSION_TASK_STACK_SIZE);
// create the magnetic calibration task (controlled by MagCalEventStruct)
_task_create_at(0, MAGCAL_TASK, 0, MagCal_task_stack, MAGCAL_TASK_STACK_SIZE);
// and this main task uses about 512 bytes stack for a grand total of 3K task stack space
// set the sensor sampling frequency (typically 200Hz)
// this is set to 200Hz by default in PE but we want to set it using value in proj_config.h
FTM_SetPeriodTicks(FTM_DeviceData, (uint16) (FTM_INCLK_HZ / SENSORFS));
// initialize globals
mqxglobals.FTMReload = (uint16)(FTM_INCLK_HZ / SENSORFS);
mqxglobals.FTMTimestamp = 0;
globals.iPacketNumber = 0;
globals.AngularVelocityPacketOn = true;
globals.DebugPacketOn = true;
globals.RPCPacketOn = true;
globals.AltPacketOn = true;
globals.iMPL3115Found = false;
globals.MagneticPacketID = 0;
// initialize the BlueRadios Bluetooth module and other user tasks
UserStartup();
// initialize the incoming command buffer to all '~' = 0x7E and trigger a callback
// when any single command character is received into the UART buffer
iCommand[0] = iCommand[1] = iCommand[2] = iCommand[3] = '~';
UART_ReceiveBlock(UART_DeviceData, sUARTInputBuf, 1);
// destroy this task (main task) now that the three new tasks are created
_task_destroy(MQX_NULL_TASK_ID);
return;
}
/**
* Call this function periodically outside the interrupt context to process commands
*/
void Command_Task()
{
if(CommandStatus != CommandStateFinishedReceiving)
return;
LED_GREEN_ClrVal();
CommandStatus = CommandStateExecuting;
uint32 intendedAddress = 0; // this is the address the packet is intended for
// uint32 intendedAddress = ( (uint32)ReceiveBuffer[1] << 24 ) | ( (uint32)ReceiveBuffer[2] << 16 ) | ( (uint32)ReceiveBuffer[3] << 8 ) | (uint32)ReceiveBuffer[4];
// intendedAddress = (intendedAddress << 8) + ReceiveBuffer[4];
// intendedAddress = (intendedAddress << 8) + ReceiveBuffer[3];
// intendedAddress = (intendedAddress << 8) + ReceiveBuffer[2];
// intendedAddress = (intendedAddress << 8) + ReceiveBuffer[1];
intendedAddress |= ReceiveBuffer[4] & 0xFF;
intendedAddress <<= 8;
intendedAddress |= ReceiveBuffer[3] & 0xFF;
intendedAddress <<= 8;
intendedAddress |= ReceiveBuffer[2] & 0xFF;
intendedAddress <<= 8;
intendedAddress |= ReceiveBuffer[1] & 0xFF;
if(intendedAddress == MyAddress || intendedAddress == 0) // 0 = broadcast
{
switch(ReceiveBuffer[5])
{
case CommandPing:
_Ping();
break;
case CommandConfigure:
_Configure(&ReceiveBuffer[6]);
break;
case CommandHomeUp:
//_HomeUp();
break;
case CommandHomeDown:
//_HomeDown();
break;
case CommandMoveTo:
_MoveTo(&ReceiveBuffer[6]);
break;
case CommandGetStatus:
_GetStatus();
break;
case CommandAssociate:
_Associate();
break;
case CommandIdentifyLed:
_IdentifyLed(ReceiveBuffer[6]);
break;
case CommandJog:
_Jog(&ReceiveBuffer[6]);
break;
case CommandResetCounters:
_ResetCounter(ReceiveBuffer[6], ReceiveBuffer[7]);
break;
case CommandDoubleMoveTo:
_DoubleMoveTo(&ReceiveBuffer[6]);
break;
case CommandSetPosGetData:
_SetPosGetData(&ReceiveBuffer[6]);
break;
case CommandGetHallPos:
_GetHallPos();
break;
case CommandGetPots:
_GetPots();
break;
case CommandGetCurremt:
_GetCurrent();
break;
}
}
CommandStatus = CommandStateWaiting;
LED_GREEN_SetVal();
}
