msg_t data_udp_send_thread(void *p) { void * arg __attribute__ ((unused)) = p; err_t err; uint8_t count = 0; struct netconn *conn; struct netbuf *buf; char* data; char msg[DATA_UDP_MSG_SIZE] ; ip_addr_t ip_addr_sensor; ip_addr_t ip_addr_fc; IMU_A_IP_ADDR(&ip_addr_sensor); IP_PSAS_FC(&ip_addr_fc); chRegSetThreadName("data_udp_send_thread"); conn = netconn_new( NETCONN_UDP ); /* Bind to the local address, or to ANY address */ // netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY) err = netconn_bind(conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port if (err == ERR_OK) { /* Connect to specific address or a broadcast address */ /* * \todo Understand why a UDP needs a connect... * This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally. * */ // netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT ); err = netconn_connect(conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A ); if(err == ERR_OK) { for( ;; ){ buf = netbuf_new(); data = netbuf_alloc(buf, sizeof(msg)); sprintf(msg, "sensor tx: %d", count++); memcpy (data, msg, sizeof (msg)); netconn_send(conn, buf); netbuf_delete(buf); // De-allocate packet buffer chThdSleepMilliseconds(500); } return RDY_OK; } else { return RDY_RESET; } } else { return RDY_RESET; } }
/*! * data_udp_rx thread. */ msg_t data_udp_receive_thread(void *p) { void * arg __attribute__ ((unused)) = p; err_t err; struct netconn *conn; ip_addr_t ip_addr_sensor; chRegSetThreadName("data_udp_receive_thread"); IMU_A_IP_ADDR(&ip_addr_sensor); /* * Create a new UDP connection handle */ conn = netconn_new(NETCONN_UDP); LWIP_ERROR("data_udp_receive_thread: invalid conn", (conn != NULL), return RDY_RESET;);
msg_t data_udp_send_thread(void *p) { void * arg __attribute__ ((unused)) = p; static const evhandler_t evhndl_mpu9150[] = { data_udp_send_mpu9150_data }; struct EventListener evl_mpu9150; err_t err; ip_addr_t ip_addr_sensor; ip_addr_t ip_addr_fc; chRegSetThreadName("data_udp_send_thread"); chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0); IMU_A_IP_ADDR(&ip_addr_sensor); IP_PSAS_FC(&ip_addr_fc); mpu9150_mac_info.conn = netconn_new( NETCONN_UDP ); /* Bind to the local address, or to ANY address */ // netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY) err = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port if (err == ERR_OK) { /* Connect to specific address or a broadcast address */ /* * \todo Understand why a UDP needs a connect... * This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally. * */ // netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT ); err = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A ); if(err == ERR_OK) { while (TRUE) { chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50))); } } return RDY_RESET; } return RDY_RESET; }
msg_t data_udp_send_thread(void *p) { void * arg __attribute__ ((unused)) = p; static const evhandler_t evhndl_imu_a[] = { data_udp_send_mpu9150_data, data_udp_send_mpl3115a2_data, data_udp_send_adis16405_data }; struct EventListener evl_mpu9150; struct EventListener evl_mpl3115a2; struct EventListener evl_adis16405; err_t err_mpu_conn; err_t err_mpl_conn; err_t err_adis_conn; ip_addr_t ip_addr_sensor; ip_addr_t ip_addr_fc; chRegSetThreadName("data_udp_send_thread"); chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0); chEvtRegister(&mpl3115a2_data_event, &evl_mpl3115a2, 1); chEvtRegister(&adis_spi_burst_data_captured, &evl_adis16405, 2); IMU_A_IP_ADDR(&ip_addr_sensor); IP_PSAS_FC(&ip_addr_fc); mpu9150_mac_info.conn = netconn_new( NETCONN_UDP ); if(mpu9150_mac_info.conn == NULL) { log_error("mpu new conn is null"); while(1); } mpl3115a2_mac_info.conn = netconn_new( NETCONN_UDP ); if(mpl3115a2_mac_info.conn == NULL) { log_error("mpl new conn is null"); while(1); } adis16405_mac_info.conn = netconn_new( NETCONN_UDP ); if(adis16405_mac_info.conn == NULL) { log_error("adis new conn is null"); while(1); } /* Bind to the local address, or to ANY address */ // netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY) err_mpu_conn = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT_MPU ); //local port if (err_mpu_conn != ERR_OK) { log_error("mpu bind is not OK"); while(1); } err_mpl_conn = netconn_bind(mpl3115a2_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT_MPL ); //local port if (err_mpl_conn != ERR_OK) { log_error("mpl bind is not OK"); while(1); } err_adis_conn = netconn_bind(adis16405_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT_ADIS ); //local port if (err_adis_conn != ERR_OK) { log_error("adis bind is not OK"); while(1); } if ((err_mpu_conn == ERR_OK) && (err_adis_conn == ERR_OK)) { /* Connect to specific address or a broadcast address */ /* * \todo Understand why a UDP needs a connect... * This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally. * */ // netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT ); err_mpu_conn = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A ); if (err_mpu_conn != ERR_OK) { log_error("mpu port connect is not OK"); while(1); } err_mpl_conn = netconn_connect(mpl3115a2_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A ); if (err_mpl_conn != ERR_OK) { log_error("mpl port connect is not OK"); while(1); } err_adis_conn = netconn_connect(adis16405_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A); if (err_adis_conn != ERR_OK) { log_error("adis port connect is not OK"); while(1); } if(err_mpu_conn == ERR_OK) { while (TRUE) { chEvtDispatch(evhndl_imu_a, chEvtWaitOneTimeout(EVENT_MASK(2)| EVENT_MASK(1)|EVENT_MASK(0), MS2ST(50))); } } else { log_error("Conn not ok"); } return RDY_RESET; } else { log_error("2 conn not ok"); } return RDY_RESET; }