void Intercore_thread()

in BalancingRobot/Software/RTOS/rtos_app/rtos_app.c [572:684]

• 105 lines of code
• 22 McCabe index (conditional complexity)

void Intercore_thread(ULONG thread_input)
{
	ULONG   actual_flags = 0;

	printf("Intercore Thread Starting\r\n");

	struct TURN_ROBOT* pTurn;
	struct SETPOINT* pSetpoint;
	struct REMOTE_CMD* pRemote;
	struct UPDATE_ACTIVE* pUpdate;

	while (true)
	{
		// wait on timer tick.
		tx_event_flags_get(&Intercore_event_flags_0, 0x1, TX_OR_CLEAR, &actual_flags, TX_WAIT_FOREVER);
		dataSize = sizeof(buf);
		int r = DequeueData(outbound, inbound, sharedBufSize, buf, &dataSize);
		if (r == 0 && dataSize > payloadStart) {
			size_t payloadBytes = dataSize - payloadStart;
			if (payloadBytes > 0) {
				switch (buf[payloadStart]) {
				case MSG_UPDATE_ACTIVE:
					pUpdate = (struct UPDATE_ACTIVE*)&buf[payloadStart];
					if (pUpdate->updateActive)
					{
						updating = true;
					}
					else
					{
						updating = false;
					}
					break;
				case MSG_REMOTE_CMD:
					pRemote=(struct REMOTE_CMD*)&buf[payloadStart];
					switch (pRemote->cmd)
					{
					case 4:	// stop.
						RemoteFwdBackAdjust = 0.0;
						RemoteRotateCommand = 0;
						break;
					case 1:	// foreward
						RemoteFwdBackAdjust = 1.5;
						RemoteRotateCommand = 0;
						break;
					case 3:	// back
						RemoteFwdBackAdjust = -1.5;
						RemoteRotateCommand = 0;
						break;
					case 2:		// right
						RemoteFwdBackAdjust = 0.0;
						RotateClockwise = false;
						RemoteRotateCommand = 2;	// right
							break;
					case 0:		// left
						RemoteFwdBackAdjust = 0.0;
						RotateClockwise = true;
						RemoteRotateCommand = 1;	// left.
						break;
					default:
						break;
					};
					break;
				case MSG_IMU_STABLE_REQUEST:
					tImuStatusMsg.id = MSG_IMU_STABLE_RESULT;
					tImuStatusMsg.imuStable = imuStable;
					EnqueueIntercoreMessage(&tImuStatusMsg, sizeof(tImuStatusMsg));
					break;
				case MSG_TELEMETRY_REQUEST:
					haveHLApp = true;		// have received at least one message from the HL App, can now start sending IMU telemetry.
					tMsg.id = MSG_DEVICE_STATUS;
					tMsg.timestamp = millis();
					tMsg.numObstaclesDetected = ToF_ObstacleCounter;
					tMsg.setpoint = CalibratedSetpoint;
					tMsg.pitch = g_pitch;
					tMsg.yaw = g_heading;
					tMsg.roll = g_roll;
					tMsg.turnNorth = TurnRobotFlag;
					tMsg.avoidActive = ObstacleDetected;
					EnqueueIntercoreMessage(&tMsg, sizeof(tMsg));
					break;
				case MSG_SETPOINT:
					pSetpoint = (struct SETPOINT*)&buf[payloadStart];
					if (pSetpoint->setpoint > 80 && pSetpoint->setpoint < 100)
					{
						CalibratedSetpoint = pSetpoint->setpoint;
					}
					break;
				case MSG_TURN_ROBOT:
					pTurn = (struct TURN_ROBOT*)&buf[payloadStart];
					
					// accept the turn if we're stood up.
					if (g_roll > 80)
					{
						if (pTurn->enabled)
						{
							RotateClockwise = GetRotationDirection(g_heading, pTurn->heading);
						}
						turnHeading = pTurn->heading;
						TurnRobotFlag = pTurn->enabled;
						if (TurnRobotFlag)
						{
							// store current heading (used in telemetry).
							turnStartHeading = g_heading;
						}
					}
					break;
				};
			}
		}
	}

	printf("Intercore Thread exit\r\n");
}