// 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;
}
예제 #2
0
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));
}
예제 #3
0
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;
}