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;
}
}
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;
BaseSequentialStream *chp = (BaseSequentialStream *)&SD1;
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_rnet;
ip_addr_t ip_addr_fc;
ip_addr_t my_ip;
uint16_t my_port;
RNET_A_IP_ADDR(&ip_addr_rnet);
IP_PSAS_FC(&ip_addr_fc);
chRegSetThreadName("data_udp_send_thread");
conn = netconn_new( NETCONN_UDP );
chThdSleepMilliseconds(1000);
chprintf(chp, "Start udp send thread\n\r");
/* 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_rnet, RNET_A_TX_PORT ); //local port
netconn_getaddr(conn, &my_ip, &my_port, local_ip);
print_ip(my_ip, my_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_RNET_A );
err = netconn_connect(conn, IP_ADDR_BROADCAST, FC_LISTEN_PORT_RNET_A );
if(err == ERR_OK) {
for( ;; ){
buf = netbuf_new();
data = netbuf_alloc(buf, sizeof(msg));
sprintf(msg, "rnet tx: %d", count++);
memcpy (data, msg, sizeof (msg));
err = netconn_send(conn, buf);
// chprintf(chp, "rnet sent: index: %d. Error: %d\r\n", count, err);
netbuf_delete(buf); // De-allocate packet buffer
chThdSleepMilliseconds(500);
}
return RDY_OK;
} else {
return RDY_RESET;
}
} else {
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;
}
