// 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;
}
void UART_OnBlockReceived(LDD_TUserData *UserDataPtr)
{
UART_Desc *ptr = (UART_Desc*)UserDataPtr;
RNG1_Put(ptr->rxChar);
UART_ReceiveBlock(ptr->handle, (LDD_TData *)&(ptr->rxChar), sizeof(ptr->rxChar));
}
void UART_OnBlockReceived(LDD_TUserData *UserDataPtr)
{
int32 isum; // 32 bit command identifier
int16 nbytes; // number of bytes received
int16 i, j; // loop counters
// this function is called when one or more characters have arrived in the UART's receive buffer
// incoming characters are placed in a delay line and processed whenever a valid command is received.
// this provides resilience against loss of incoming characters.
// note also that although this callback is theoretically called whenever a single byte is received,
// in practice there may be bursts of more than one byte in the receive buffer.
// all received bytes are processed before this callback is executed.
// determine how many bytes are available in the UART receive buffer
nbytes = UART_GetReceivedDataNum(UART_DeviceData);
// parse all received bytes in sUARTInputBuf into the iCommand delay line
for (i = 0; i < nbytes; i++)
{
// shuffle the iCommand delay line and add the new command byte
for (j = 0; j < 3; j++)
iCommand[j] = iCommand[j + 1];
iCommand[3] = sUARTInputBuf[i];
// check if we have a valid command yet
isum = ((((((int32)iCommand[0] << 8) + iCommand[1]) << 8) + iCommand[2]) << 8) + iCommand[3];
switch (isum)
{
// "VG+ " = enable angular velocity packet transmission
case ((((('V' << 8) + 'G') << 8) + '+') << 8) + ' ':
globals.AngularVelocityPacketOn = true;
iCommand[3] = '~';
break;
// "VG- " = disable angular velocity packet transmission
case ((((('V' << 8) + 'G') << 8) + '-') << 8) + ' ':
globals.AngularVelocityPacketOn = false;
iCommand[3] = '~';
break;
// "DB+ " = enable debug packet transmission
case ((((('D' << 8) + 'B') << 8) + '+') << 8) + ' ':
globals.DebugPacketOn = true;
iCommand[3] = '~';
break;
// "DB- " = disable debug packet transmission
case ((((('D' << 8) + 'B') << 8) + '-') << 8) + ' ':
globals.DebugPacketOn = false;
iCommand[3] = '~';
break;
// "Q3 " = transmit 3-axis accelerometer quaternion in standard packet
case ((((('Q' << 8) + '3') << 8) + ' ') << 8) + ' ':
#if defined COMPUTE_3DOF_G_BASIC
globals.QuaternionPacketType = Q3;
iCommand[3] = '~';
#endif
break;
// "Q3M " = transmit 3-axis magnetometer quaternion in standard packet
case ((((('Q' << 8) + '3') << 8) + 'M') << 8) + ' ':
#if defined COMPUTE_3DOF_B_BASIC
globals.QuaternionPacketType = Q3M;
iCommand[3] = '~';
#endif
break;
// "Q3G " = transmit 3-axis gyro quaternion in standard packet
case ((((('Q' << 8) + '3') << 8) + 'G') << 8) + ' ':
#if defined COMPUTE_3DOF_Y_BASIC
globals.QuaternionPacketType = Q3G;
iCommand[3] = '~';
#endif
break;
// "Q6MA" = transmit 6-axis mag/accel quaternion in standard packet
case ((((('Q' << 8) + '6') << 8) + 'M') << 8) + 'A':
#if defined COMPUTE_6DOF_GB_BASIC
globals.QuaternionPacketType = Q6MA;
iCommand[3] = '~';
#endif
break;
// "Q6AG" = transmit 6-axis accel/gyro quaternion in standard packet
case ((((('Q' << 8) + '6') << 8) + 'A') << 8) + 'G':
#if defined COMPUTE_6DOF_GY_KALMAN
globals.QuaternionPacketType = Q6AG;
iCommand[3] = '~';
#endif
break;
// "Q9 " = transmit 9-axis quaternion in standard packet (default)
case ((((('Q' << 8) + '9') << 8) + ' ') << 8) + ' ':
#if defined COMPUTE_9DOF_GBY_KALMAN
globals.QuaternionPacketType = Q9;
iCommand[3] = '~';
#endif
break;
// "RPC+" = Roll/Pitch/Compass on
case ((((('R' << 8) + 'P') << 8) + 'C') << 8) + '+':
globals.RPCPacketOn = true;
iCommand[3] = '~';
break;
// "RPC-" = Roll/Pitch/Compass off
case ((((('R' << 8) + 'P') << 8) + 'C') << 8) + '-':
globals.RPCPacketOn = false;
iCommand[3] = '~';
break;
// "ALT+" = Altitude packet on
case ((((('A' << 8) + 'L') << 8) + 'T') << 8) + '+':
globals.AltPacketOn = true;
iCommand[3] = '~';
break;
// "ALT-" = Altitude packet off
case ((((('A' << 8) + 'L') << 8) + 'T') << 8) + '-':
globals.AltPacketOn = false;
iCommand[3] = '~';
break;
// "RST " = Soft reset
case ((((('R' << 8) + 'S') << 8) + 'T') << 8) + ' ':
Fusion_Init();
mqxglobals.FTMTimestamp = 0;
iCommand[3] = '~';
break;
default:
// no action
break;
}
} // end of loop over received characters
// generate the next callback event to this function when the next character arrives
// this function is non-blocking
UART_ReceiveBlock(UART_DeviceData, sUARTInputBuf, 1);
return;
}