54 printf(
"Starting new connection to the State Server\r\n");
56 ATTEMPT_STATE_CONNECTION:
67 if (connectionIndex == MAX_STATE_CONNECTIONS)
71 puts(
"Too many State server connections... not accepting last attempt.");
77 puts(
"Too many State server connections... closing old connection.");
79 goto ATTEMPT_STATE_CONNECTION;
84 mpSetsockopt(sd, SOL_SOCKET, SO_KEEPALIVE, (
char*)&sockOpt,
sizeof(sockOpt));
87 controller->
tidStateSendState[connectionIndex] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE,
89 (
int)controller, connectionIndex, 0, 0, 0, 0, 0, 0, 0, 0);
92 if (controller->tidStateSendState[connectionIndex] !=
INVALID_TASK)
96 mpSetAlarm(8004,
"MOTOROS FAILED TO CREATE TASK", 3);
137 int msgSize, fexMsgSize = 0;
138 BOOL bOkToSendExFeedback;
141 printf(
"Starting State Server Send State task\r\n");
142 printf(
"Controller number of group = %d\r\n", controller->
numGroup);
147 bOkToSendExFeedback =
TRUE;
150 for(groupNo=0; groupNo < controller->
numGroup; groupNo++)
162 printf(
"Ros_SimpleMsg_JointFeedback returned a message size of 0\r\n");
163 bOkToSendExFeedback =
FALSE;
168 bOkToSendExFeedback =
FALSE;
170 if (bOkToSendExFeedback)
188 printf(
"State Server Send State task was terminated\r\n");
201 ret = mpSend(controller->
sdStateConnections[connectionIndex], (
char*)(sendMsg), msgSize, 0);
204 printf(
"StateServer Send failure. Closing state server connection.\r\n");
void Ros_Controller_SetIOState(ULONG signal, BOOL status)
int tidStateSendState[MAX_STATE_CONNECTIONS]
void Ros_StateServer_SendState(Controller *controller, int connectionIndex)
CtrlGroup * ctrlGroups[MP_GRP_NUM]
#define MAX_STATE_CONNECTIONS
int Ros_SimpleMsg_JointFeedbackEx_Build(int groupIndex, SimpleMsg *src_msgFeedback, SimpleMsg *dst_msgExtendedFeedback)
BOOL Ros_StateServer_SendMsgToAllClient(Controller *controller, int connectionIndex, SimpleMsg *sendMsg, int msgSize)
void Ros_Sleep(float milliseconds)
int sdStateConnections[MAX_STATE_CONNECTIONS]
int Ros_Controller_StatusToMsg(Controller *controller, SimpleMsg *sendMsg)
#define IO_FEEDBACK_STATESERVERCONNECTED
#define STATE_UPDATE_MIN_PERIOD
void Ros_SimpleMsg_JointFeedbackEx_Init(int numberOfGroups, SimpleMsg *sendMsg)
void Ros_StateServer_StopConnection(Controller *controller, int connectionIndex)
BOOL Ros_MotionServer_HasDataInQueue(Controller *controller)
int Ros_SimpleMsg_JointFeedback(CtrlGroup *ctrlGroup, SimpleMsg *sendMsg)
void Ros_StateServer_StartNewConnection(Controller *controller, int sd)