void XMLRPCServer::UDPreceive(void* params)
{
struct netconn *conn;
struct netbuf *buf;
err_t err;
// Initialize memory (in stack) for message.
char message[60]; // TODO: Set its size according to UDPMessage.
os_printf("Test!\n");
conn = netconn_new(NETCONN_UDP);
for (;;) {
// Check if connection was created successfully.
if (conn != NULL) {
err = netconn_bind(conn, IP_ADDR_ANY, UDP_RECEIVE_PORT);
// Check if we were able to bind to port.
if (err == ERR_OK) {
portTickType xLastWakeTime;
// Initialize the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
// Start periodic loop.
while (1) {
buf = netconn_recv(conn);
if (buf != NULL) {
struct ip_addr* ip;
uint16_t port;
ip = buf->addr;
port = buf->port;
//if(ip != NULL)
//os_printf("Received from %d:%d!\n", ip->addr, port);
// Copy received data into message.
uint16_t len = netbuf_len(buf);
if (len > 15) {
netbuf_copy (buf, &message, len);
uint32_t connectionID = *((uint32_t*)&message[0]);
TopicReader* tr = getTopicReader(connectionID);
if (tr != NULL) {
tr->enqueueMessage(&message[8]);
//os_printf("ConnectionID: %d, topic:%s\n", connectionID, tr->getTopic());
}
}
// Deallocate previously created memory.
netbuf_delete(buf);
}
// Use delay until to guarantee periodic execution of each loop iteration.
else {
os_printf("buf = NULL!\n");
vTaskDelayUntil(&xLastWakeTime, 30);
}
}
} else {
os_printf("cannot bind netconn\n");
}
} else {
os_printf("cannot create new UDP netconn\n");
conn = netconn_new(NETCONN_UDP);
}
// If connection failed, wait for 50 ms before retrying.
vTaskDelay(50);
}
}
void XMLRPCServer::UDPSend(void* params)
{
UDPHandler* uh = UDPHandler::instance();
struct netconn* conn = netconn_new( NETCONN_UDP );
netconn_bind(conn, IP_ADDR_ANY, UDP_LOCAL_PORT);
static uint8_t counter = 1;
pinMode(GPIO_PD11, OUTPUT);
UDPMessage msg;
struct ip_addr ip;
ip.addr = inet_addr(ROS_MASTER_IP);
for (;;) {
//digitalWrite(GPIO_PD11, HIGH);
digitalWrite(GPIO_PD11, pin3);
uh->dequeueMessage(&msg);
//digitalWrite(GPIO_PD11, LOW);
// inter-node communication
TopicReader* tr = getTopicReader(msg.topic);
if (tr != NULL) {
tr->enqueueMessage(msg.data);
}
TopicWriter* tw = getTopicWriter(msg.topic);
if (tw != NULL) {
UDPConnection* const* connections = tw->getConnections();
if (connections != NULL) {
for (int i = 0; i < MAX_UDP_CONNECTIONS; i++) {
const UDPConnection* connection = connections[i];
if (connection && connection->isValid()) {
err_t err = netconn_connect(conn, &ip, connection->getPort());
//os_printf("1Port: %d LWIP Error:%d\n", endpoint.port, err);
os_printf("Connecting %s:%d, err:%d\n", ip, connection->getPort(), err);
struct netbuf *buf = netbuf_new();
char msgHeader[8];
uint32_t connectionID = connection->getID();
memcpy(&msgHeader[0], &connectionID, sizeof(uint32_t));
msgHeader[4] = 0;
msgHeader[5] = counter++;
msgHeader[6] = 0x01;
msgHeader[7] = 0;
uint32_t msgLen = *((uint32_t*) msg.data) + 4;
void* data = netbuf_alloc(buf, msgLen + sizeof(msgHeader)); // Also deallocated with netbuf_delete(buf)
if (data != NULL) {
memcpy (data, msgHeader, sizeof(msgHeader));
memcpy (data + sizeof(msgHeader), msg.data, msgLen);
} else {
os_printf("XMLRPCServer::UDPSend data is NULL!\n");
}
err = netconn_send(conn, buf);
//os_printf("2Port: %d LWIP Error:%d\n", endpoint.port, err);
netbuf_delete(buf);
}
}
}
}
pin3 = !pin3;
}
}
