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; } }