int main(int argc, char **argv)
{
// Set the default tracking system position and angle
struct EcefCoor_d tracking_ecef;
//alt 45 m because of ellipsoid altitude in Delft
tracking_ecef.x = 3924331.5;
tracking_ecef.y = 300361.7;
tracking_ecef.z = 5002197.1;
tracking_offset_angle = 33.0 / 57.6;
ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
// Parse the options from cmdline
parse_options(argc, argv);
printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat),
DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
// Create the network connections
printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n",
natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving
udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);
udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB
printf_debug("Starting NatNet command socket (server address: %s, command port: %d)\n", natnet_addr, natnet_cmd_port);
udp_socket_create(&natnet_cmd, natnet_addr, natnet_cmd_port, 0, 1);
udp_socket_set_recvbuf(&natnet_cmd, 0x100000); // 1MB
// Create the Ivy Client
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0);
IvyStart(ivy_bus);
// Create the main timers
printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n",
freq_transmit, min_velocity_samples);
g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL);
GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd);
g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP,
sample_data, NULL);
// Run the main loop
g_main_loop_run(ml);
return 0;
}
/**
* Initialize the view video
*/
void viewvideo_init(void)
{
char save_name[512];
struct video_listener *listener = cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function, VIEWVIDEO_NICE_LEVEL);
listener->maximum_fps = VIEWVIDEO_FPS;
viewvideo.is_streaming = true;
#if VIEWVIDEO_USE_NETCAT
// Create an Netcat receiver file for the streaming
sprintf(save_name, "%s/netcat-recv.sh", STRINGIFY(VIEWVIDEO_SHOT_PATH));
FILE *fp = fopen(save_name, "w");
if (fp != NULL) {
fprintf(fp, "i=0\n");
fprintf(fp, "while true\n");
fprintf(fp, "do\n");
fprintf(fp, "\tn=$(printf \"%%04d\" $i)\n");
fprintf(fp, "\tnc -l 0.0.0.0 %d > img_${n}.jpg\n", (int)(VIEWVIDEO_PORT_OUT));
fprintf(fp, "\ti=$((i+1))\n");
fprintf(fp, "done\n");
fclose(fp);
} else {
printf("[viewvideo] Failed to create netcat receiver file.\n");
}
#else
// Open udp socket
udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
// Create an SDP file for the streaming
sprintf(save_name, "%s/stream.sdp", STRINGIFY(VIEWVIDEO_SHOT_PATH));
FILE *fp = fopen(save_name, "w");
if (fp != NULL) {
fprintf(fp, "v=0\n");
fprintf(fp, "m=video %d RTP/AVP 26\n", (int)(VIEWVIDEO_PORT_OUT));
fprintf(fp, "c=IN IP4 0.0.0.0\n");
fclose(fp);
} else {
printf("[viewvideo] Failed to create SDP file.\n");
}
#endif
}
/* return non zero if error */
static int udp_open(URLContext *h, const char *uri, int flags)
{
char hostname[1024];
int port, udp_fd = -1, tmp, bind_ret = -1;
UDPContext *s = NULL;
int is_output;
const char *p;
char buf[256];
#if !CONFIG_IPV6
struct sockaddr_in my_addr;
#else
struct sockaddr_storage my_addr;
#endif
int len;
h->is_streamed = 1;
h->max_packet_size = 1472;
is_output = (flags & URL_WRONLY);
if(!ff_network_init())
return AVERROR(EIO);
s = av_mallocz(sizeof(UDPContext));
if (!s)
return AVERROR(ENOMEM);
h->priv_data = s;
s->ttl = 16;
s->buffer_size = is_output ? UDP_TX_BUF_SIZE : UDP_MAX_PKT_SIZE;
p = strchr(uri, '?');
if (p) {
s->reuse_socket = find_info_tag(buf, sizeof(buf), "reuse", p);
if (find_info_tag(buf, sizeof(buf), "ttl", p)) {
s->ttl = strtol(buf, NULL, 10);
}
if (find_info_tag(buf, sizeof(buf), "localport", p)) {
s->local_port = strtol(buf, NULL, 10);
}
if (find_info_tag(buf, sizeof(buf), "pkt_size", p)) {
h->max_packet_size = strtol(buf, NULL, 10);
}
if (find_info_tag(buf, sizeof(buf), "buffer_size", p)) {
s->buffer_size = strtol(buf, NULL, 10);
}
}
/* fill the dest addr */
url_split(NULL, 0, NULL, 0, hostname, sizeof(hostname), &port, NULL, 0, uri);
/* XXX: fix url_split */
if (hostname[0] == '\0' || hostname[0] == '?') {
/* only accepts null hostname if input */
if (flags & URL_WRONLY)
goto fail;
} else {
udp_set_remote_url(h, uri);
}
if (s->is_multicast && !(h->flags & URL_WRONLY))
s->local_port = port;
udp_fd = udp_socket_create(s, &my_addr, &len);
if (udp_fd < 0)
goto fail;
if (s->reuse_socket)
if (setsockopt (udp_fd, SOL_SOCKET, SO_REUSEADDR, &(s->reuse_socket), sizeof(s->reuse_socket)) != 0)
goto fail;
/* the bind is needed to give a port to the socket now */
/* if multicast, try the multicast address bind first */
if (s->is_multicast && !(h->flags & URL_WRONLY)) {
bind_ret = bind(udp_fd,(struct sockaddr *)&s->dest_addr, len);
}
/* bind to the local address if not multicast or if the multicast
* bind failed */
if (bind_ret < 0 && bind(udp_fd,(struct sockaddr *)&my_addr, len) < 0)
goto fail;
len = sizeof(my_addr);
getsockname(udp_fd, (struct sockaddr *)&my_addr, &len);
s->local_port = udp_port(&my_addr, len);
if (s->is_multicast) {
if (h->flags & URL_WRONLY) {
/* output */
if (udp_set_multicast_ttl(udp_fd, s->ttl, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
} else {
/* input */
if (udp_join_multicast_group(udp_fd, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
}
}
if (is_output) {
/* limit the tx buf size to limit latency */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_SNDBUF, &tmp, sizeof(tmp)) < 0) {
av_log(NULL, AV_LOG_ERROR, "setsockopt(SO_SNDBUF): %s\n", strerror(errno));
goto fail;
}
} else {
/* set udp recv buffer size to the largest possible udp packet size to
* avoid losing data on OSes that set this too low by default. */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_RCVBUF, &tmp, sizeof(tmp)) < 0) {
av_log(NULL, AV_LOG_WARNING, "setsockopt(SO_RECVBUF): %s\n", strerror(errno));
}
/* make the socket non-blocking */
ff_socket_nonblock(udp_fd, 1);
}
s->udp_fd = udp_fd;
return 0;
fail:
if (udp_fd >= 0)
closesocket(udp_fd);
av_free(s);
return AVERROR(EIO);
}
/* return non zero if error */
static int udp_open(URLContext *h, const char *uri, int flags)
{
char hostname[1024];
int port, udp_fd = -1, tmp, bind_ret = -1;
UDPContext *s = NULL;
int is_output;
const char *p;
char buf[256];
struct sockaddr_storage my_addr;
int len;
int reuse_specified = 0;
h->is_streamed = 1;
h->max_packet_size = 1472;
is_output = !(flags & AVIO_FLAG_READ);
s = av_mallocz(sizeof(UDPContext));
if (!s)
return AVERROR(ENOMEM);
h->priv_data = s;
s->ttl = 16;
s->buffer_size = is_output ? UDP_TX_BUF_SIZE : UDP_MAX_PKT_SIZE;
s->circular_buffer_size = 7*188*4096;
p = strchr(uri, '?');
if (p) {
if (av_find_info_tag(buf, sizeof(buf), "reuse", p)) {
char *endptr=NULL;
s->reuse_socket = strtol(buf, &endptr, 10);
/* assume if no digits were found it is a request to enable it */
if (buf == endptr)
s->reuse_socket = 1;
reuse_specified = 1;
}
if (av_find_info_tag(buf, sizeof(buf), "ttl", p)) {
s->ttl = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "localport", p)) {
s->local_port = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "pkt_size", p)) {
h->max_packet_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "buffer_size", p)) {
s->buffer_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "connect", p)) {
s->is_connected = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "buf_size", p)) {
s->circular_buffer_size = strtol(buf, NULL, 10)*188;
}
}
/* fill the dest addr */
av_url_split(NULL, 0, NULL, 0, hostname, sizeof(hostname), &port, NULL, 0, uri);
/* XXX: fix av_url_split */
if (hostname[0] == '\0' || hostname[0] == '?') {
/* only accepts null hostname if input */
if (!(flags & AVIO_FLAG_READ))
goto fail;
} else {
if (ff_udp_set_remote_url(h, uri) < 0)
goto fail;
}
if (s->is_multicast && (h->flags & AVIO_FLAG_READ))
s->local_port = port;
udp_fd = udp_socket_create(s, &my_addr, &len);
if (udp_fd < 0)
goto fail;
/* Follow the requested reuse option, unless it's multicast in which
* case enable reuse unless explicitely disabled.
*/
if (s->reuse_socket || (s->is_multicast && !reuse_specified)) {
s->reuse_socket = 1;
if (setsockopt (udp_fd, SOL_SOCKET, SO_REUSEADDR, &(s->reuse_socket), sizeof(s->reuse_socket)) != 0)
goto fail;
}
/* the bind is needed to give a port to the socket now */
/* if multicast, try the multicast address bind first */
if (s->is_multicast && (h->flags & AVIO_FLAG_READ)) {
bind_ret = bind(udp_fd,(struct sockaddr *)&s->dest_addr, len);
}
/* bind to the local address if not multicast or if the multicast
* bind failed */
if (bind_ret < 0 && bind(udp_fd,(struct sockaddr *)&my_addr, len) < 0)
goto fail;
len = sizeof(my_addr);
getsockname(udp_fd, (struct sockaddr *)&my_addr, &len);
s->local_port = udp_port(&my_addr, len);
if (s->is_multicast) {
if (!(h->flags & AVIO_FLAG_READ)) {
/* output */
if (udp_set_multicast_ttl(udp_fd, s->ttl, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
} else {
/* input */
if (udp_join_multicast_group(udp_fd, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
}
}
if (is_output) {
/* limit the tx buf size to limit latency */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_SNDBUF, &tmp, sizeof(tmp)) < 0) {
av_log(h, AV_LOG_ERROR, "setsockopt(SO_SNDBUF): %s\n", strerror(errno));
goto fail;
}
} else {
/* set udp recv buffer size to the largest possible udp packet size to
* avoid losing data on OSes that set this too low by default. */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_RCVBUF, &tmp, sizeof(tmp)) < 0) {
av_log(h, AV_LOG_WARNING, "setsockopt(SO_RECVBUF): %s\n", strerror(errno));
}
/* make the socket non-blocking */
ff_socket_nonblock(udp_fd, 1);
}
if (s->is_connected) {
if (connect(udp_fd, (struct sockaddr *) &s->dest_addr, s->dest_addr_len)) {
av_log(h, AV_LOG_ERROR, "connect: %s\n", strerror(errno));
goto fail;
}
}
s->udp_fd = udp_fd;
#if 0 && HAVE_PTHREADS
if (!is_output && s->circular_buffer_size) {
/* start the task going */
s->fifo = av_fifo_alloc(s->circular_buffer_size);
if (pthread_create(&s->circular_buffer_thread, NULL, circular_buffer_task, h)) {
av_log(h, AV_LOG_ERROR, "pthread_create failed\n");
goto fail;
}
}
#endif
return 0;
fail:
if (udp_fd >= 0)
closesocket(udp_fd);
av_fifo_free(s->fifo);
av_free(s);
return AVERROR(EIO);
}
/* return non zero if error */
static int udp_open(URLContext *h, const char *uri, int flags)
{
char hostname[1024], localaddr[1024] = "";
int port, udp_fd = -1, tmp, bind_ret = -1, dscp = -1;
UDPContext *s = h->priv_data;
int is_output;
const char *p;
char buf[256];
struct sockaddr_storage my_addr;
socklen_t len;
int i, num_include_sources = 0, num_exclude_sources = 0;
char *include_sources[32], *exclude_sources[32];
h->is_streamed = 1;
is_output = !(flags & AVIO_FLAG_READ);
if (s->buffer_size < 0)
s->buffer_size = is_output ? UDP_TX_BUF_SIZE : UDP_MAX_PKT_SIZE;
if (s->sources) {
if (parse_source_list(s->sources, include_sources,
&num_include_sources,
FF_ARRAY_ELEMS(include_sources)))
goto fail;
}
if (s->block) {
if (parse_source_list(s->block, exclude_sources, &num_exclude_sources,
FF_ARRAY_ELEMS(exclude_sources)))
goto fail;
}
if (s->pkt_size > 0)
h->max_packet_size = s->pkt_size;
p = strchr(uri, '?');
if (p) {
if (av_find_info_tag(buf, sizeof(buf), "reuse", p)) {
char *endptr = NULL;
s->reuse_socket = strtol(buf, &endptr, 10);
/* assume if no digits were found it is a request to enable it */
if (buf == endptr)
s->reuse_socket = 1;
}
if (av_find_info_tag(buf, sizeof(buf), "overrun_nonfatal", p)) {
char *endptr = NULL;
s->overrun_nonfatal = strtol(buf, &endptr, 10);
/* assume if no digits were found it is a request to enable it */
if (buf == endptr)
s->overrun_nonfatal = 1;
if (!HAVE_PTHREAD_CANCEL)
av_log(h, AV_LOG_WARNING,
"'overrun_nonfatal' option was set but it is not supported "
"on this build (pthread support is required)\n");
}
if (av_find_info_tag(buf, sizeof(buf), "ttl", p)) {
s->ttl = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "udplite_coverage", p)) {
s->udplite_coverage = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "localport", p)) {
s->local_port = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "pkt_size", p)) {
s->pkt_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "buffer_size", p)) {
s->buffer_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "connect", p)) {
s->is_connected = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "dscp", p)) {
dscp = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "fifo_size", p)) {
s->circular_buffer_size = strtol(buf, NULL, 10);
if (!HAVE_PTHREAD_CANCEL)
av_log(h, AV_LOG_WARNING,
"'circular_buffer_size' option was set but it is not supported "
"on this build (pthread support is required)\n");
}
if (av_find_info_tag(buf, sizeof(buf), "localaddr", p)) {
av_strlcpy(localaddr, buf, sizeof(localaddr));
}
if (av_find_info_tag(buf, sizeof(buf), "sources", p)) {
if (parse_source_list(buf, include_sources, &num_include_sources,
FF_ARRAY_ELEMS(include_sources)))
goto fail;
}
if (av_find_info_tag(buf, sizeof(buf), "block", p)) {
if (parse_source_list(buf, exclude_sources, &num_exclude_sources,
FF_ARRAY_ELEMS(exclude_sources)))
goto fail;
}
if (!is_output && av_find_info_tag(buf, sizeof(buf), "timeout", p))
s->timeout = strtol(buf, NULL, 10);
if (is_output && av_find_info_tag(buf, sizeof(buf), "broadcast", p))
s->is_broadcast = strtol(buf, NULL, 10);
}
/* handling needed to support options picking from both AVOption and URL */
s->circular_buffer_size *= 188;
if (flags & AVIO_FLAG_WRITE) {
h->max_packet_size = s->pkt_size;
} else {
h->max_packet_size = UDP_MAX_PKT_SIZE;
}
h->rw_timeout = s->timeout;
/* fill the dest addr */
av_url_split(NULL, 0, NULL, 0, hostname, sizeof(hostname), &port, NULL, 0, uri);
/* XXX: fix av_url_split */
if (hostname[0] == '\0' || hostname[0] == '?') {
/* only accepts null hostname if input */
if (!(flags & AVIO_FLAG_READ))
goto fail;
} else {
if (ff_udp_set_remote_url(h, uri) < 0)
goto fail;
}
if ((s->is_multicast || s->local_port <= 0) && (h->flags & AVIO_FLAG_READ))
s->local_port = port;
if (localaddr[0])
udp_fd = udp_socket_create(s, &my_addr, &len, localaddr);
else
udp_fd = udp_socket_create(s, &my_addr, &len, s->localaddr);
if (udp_fd < 0)
goto fail;
s->local_addr_storage=my_addr; //store for future multicast join
/* Follow the requested reuse option, unless it's multicast in which
* case enable reuse unless explicitly disabled.
*/
if (s->reuse_socket > 0 || (s->is_multicast && s->reuse_socket < 0)) {
s->reuse_socket = 1;
if (setsockopt (udp_fd, SOL_SOCKET, SO_REUSEADDR, &(s->reuse_socket), sizeof(s->reuse_socket)) != 0)
goto fail;
}
if (s->is_broadcast) {
#ifdef SO_BROADCAST
if (setsockopt (udp_fd, SOL_SOCKET, SO_BROADCAST, &(s->is_broadcast), sizeof(s->is_broadcast)) != 0)
#endif
goto fail;
}
/* Set the checksum coverage for UDP-Lite (RFC 3828) for sending and receiving.
* The receiver coverage has to be less than or equal to the sender coverage.
* Otherwise, the receiver will drop all packets.
*/
if (s->udplite_coverage) {
if (setsockopt (udp_fd, IPPROTO_UDPLITE, UDPLITE_SEND_CSCOV, &(s->udplite_coverage), sizeof(s->udplite_coverage)) != 0)
av_log(h, AV_LOG_WARNING, "socket option UDPLITE_SEND_CSCOV not available");
if (setsockopt (udp_fd, IPPROTO_UDPLITE, UDPLITE_RECV_CSCOV, &(s->udplite_coverage), sizeof(s->udplite_coverage)) != 0)
av_log(h, AV_LOG_WARNING, "socket option UDPLITE_RECV_CSCOV not available");
}
if (dscp >= 0) {
dscp <<= 2;
if (setsockopt (udp_fd, IPPROTO_IP, IP_TOS, &dscp, sizeof(dscp)) != 0)
goto fail;
}
/* If multicast, try binding the multicast address first, to avoid
* receiving UDP packets from other sources aimed at the same UDP
* port. This fails on windows. This makes sending to the same address
* using sendto() fail, so only do it if we're opened in read-only mode. */
if (s->is_multicast && !(h->flags & AVIO_FLAG_WRITE)) {
bind_ret = bind(udp_fd,(struct sockaddr *)&s->dest_addr, len);
}
/* bind to the local address if not multicast or if the multicast
* bind failed */
/* the bind is needed to give a port to the socket now */
if (bind_ret < 0 && bind(udp_fd,(struct sockaddr *)&my_addr, len) < 0) {
log_net_error(h, AV_LOG_ERROR, "bind failed");
goto fail;
}
len = sizeof(my_addr);
getsockname(udp_fd, (struct sockaddr *)&my_addr, &len);
s->local_port = udp_port(&my_addr, len);
if (s->is_multicast) {
if (h->flags & AVIO_FLAG_WRITE) {
/* output */
if (udp_set_multicast_ttl(udp_fd, s->ttl, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
}
if (h->flags & AVIO_FLAG_READ) {
/* input */
if (num_include_sources && num_exclude_sources) {
av_log(h, AV_LOG_ERROR, "Simultaneously including and excluding multicast sources is not supported\n");
goto fail;
}
if (num_include_sources) {
if (udp_set_multicast_sources(udp_fd, (struct sockaddr *)&s->dest_addr, s->dest_addr_len, include_sources, num_include_sources, 1) < 0)
goto fail;
} else {
if (udp_join_multicast_group(udp_fd, (struct sockaddr *)&s->dest_addr,(struct sockaddr *)&s->local_addr_storage) < 0)
goto fail;
}
if (num_exclude_sources) {
if (udp_set_multicast_sources(udp_fd, (struct sockaddr *)&s->dest_addr, s->dest_addr_len, exclude_sources, num_exclude_sources, 0) < 0)
goto fail;
}
}
}
if (is_output) {
/* limit the tx buf size to limit latency */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_SNDBUF, &tmp, sizeof(tmp)) < 0) {
log_net_error(h, AV_LOG_ERROR, "setsockopt(SO_SNDBUF)");
goto fail;
}
} else {
/* set udp recv buffer size to the requested value (default 64K) */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_RCVBUF, &tmp, sizeof(tmp)) < 0) {
log_net_error(h, AV_LOG_WARNING, "setsockopt(SO_RECVBUF)");
}
len = sizeof(tmp);
if (getsockopt(udp_fd, SOL_SOCKET, SO_RCVBUF, &tmp, &len) < 0) {
log_net_error(h, AV_LOG_WARNING, "getsockopt(SO_RCVBUF)");
} else {
av_log(h, AV_LOG_DEBUG, "end receive buffer size reported is %d\n", tmp);
if(tmp < s->buffer_size)
av_log(h, AV_LOG_WARNING, "attempted to set receive buffer to size %d but it only ended up set as %d", s->buffer_size, tmp);
}
/* make the socket non-blocking */
ff_socket_nonblock(udp_fd, 1);
}
if (s->is_connected) {
if (connect(udp_fd, (struct sockaddr *) &s->dest_addr, s->dest_addr_len)) {
log_net_error(h, AV_LOG_ERROR, "connect");
goto fail;
}
}
for (i = 0; i < num_include_sources; i++)
av_freep(&include_sources[i]);
for (i = 0; i < num_exclude_sources; i++)
av_freep(&exclude_sources[i]);
s->udp_fd = udp_fd;
#if HAVE_PTHREAD_CANCEL
if (!is_output && s->circular_buffer_size) {
int ret;
/* start the task going */
s->fifo = av_fifo_alloc(s->circular_buffer_size);
ret = pthread_mutex_init(&s->mutex, NULL);
if (ret != 0) {
av_log(h, AV_LOG_ERROR, "pthread_mutex_init failed : %s\n", strerror(ret));
goto fail;
}
ret = pthread_cond_init(&s->cond, NULL);
if (ret != 0) {
av_log(h, AV_LOG_ERROR, "pthread_cond_init failed : %s\n", strerror(ret));
goto cond_fail;
}
ret = pthread_create(&s->circular_buffer_thread, NULL, circular_buffer_task, h);
if (ret != 0) {
av_log(h, AV_LOG_ERROR, "pthread_create failed : %s\n", strerror(ret));
goto thread_fail;
}
s->thread_started = 1;
}
#endif
return 0;
#if HAVE_PTHREAD_CANCEL
thread_fail:
pthread_cond_destroy(&s->cond);
cond_fail:
pthread_mutex_destroy(&s->mutex);
#endif
fail:
if (udp_fd >= 0)
closesocket(udp_fd);
av_fifo_freep(&s->fifo);
for (i = 0; i < num_include_sources; i++)
av_freep(&include_sources[i]);
for (i = 0; i < num_exclude_sources; i++)
av_freep(&exclude_sources[i]);
return AVERROR(EIO);
}
int udp_open( hnd_t *p_handle, obe_udp_opts_t *udp_opts )
{
int udp_fd = -1, tmp, bind_ret = -1;
struct sockaddr_storage my_addr;
int len;
obe_udp_ctx *s = calloc( 1, sizeof(*s) );
*p_handle = NULL;
if( !s )
return -1;
strncpy( s->hostname, udp_opts->hostname, sizeof(s->hostname) );
s->port = udp_opts->port;
s->local_port = udp_opts->local_port;
s->reuse_socket = udp_opts->reuse_socket;
s->ttl = udp_opts->ttl;
s->buffer_size = udp_opts->buffer_size;
s->miface = udp_opts->miface;
if( udp_set_remote_url( s ) < 0 )
goto fail;
udp_fd = udp_socket_create( s, &my_addr, &len );
if( udp_fd < 0 )
goto fail;
if( s->reuse_socket || s->is_multicast )
{
s->reuse_socket = 1;
if( setsockopt( udp_fd, SOL_SOCKET, SO_REUSEADDR, &(s->reuse_socket), sizeof(s->reuse_socket) ) != 0)
goto fail;
}
/* bind to the local address if not multicast or if the multicast
* bind failed */
if( bind_ret < 0 && bind( udp_fd, (struct sockaddr *)&my_addr, len ) < 0 )
goto fail;
len = sizeof(my_addr);
getsockname( udp_fd, (struct sockaddr *)&my_addr, (socklen_t *) &len );
s->local_port = udp_port( &my_addr, len );
/* set output multicast ttl */
if( s->is_multicast && udp_set_multicast_opts( udp_fd, s ) < 0 )
goto fail;
/* limit the tx buf size to limit latency */
tmp = s->buffer_size;
if( setsockopt( udp_fd, SOL_SOCKET, SO_SNDBUF, &tmp, sizeof(tmp) ) < 0 )
goto fail;
if( s->is_connected && connect( udp_fd, (struct sockaddr *)&s->dest_addr, s->dest_addr_len ) )
goto fail;
s->udp_fd = udp_fd;
*p_handle = s;
return 0;
fail:
if( udp_fd >= 0 )
close( udp_fd );
free( s );
return -1;
}
int main(void)
{
i2c_bus_t bus;
int ret = i2c_bus_open(&bus, "/dev/i2c-3");
if (ret < 0)
{
fatal("could not open i2c bus", ret);
return EXIT_FAILURE;
}
/* ITG: */
itg3200_dev_t itg;
itg_again:
ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
if (ret < 0)
{
fatal("could not inizialize ITG3200", ret);
if (ret == -EAGAIN)
{
goto itg_again;
}
return EXIT_FAILURE;
}
/* BMA: */
bma180_dev_t bma;
bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);
/* HMC: */
hmc5883_dev_t hmc;
hmc5883_init(&hmc, &bus);
/* MS: */
ms5611_dev_t ms;
ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
if (ret < 0)
{
fatal("could not inizialize MS5611", ret);
return EXIT_FAILURE;
}
pthread_t thread;
pthread_create(&thread, NULL, ms5611_reader, &ms);
/* initialize AHRS filter: */
madgwick_ahrs_t madgwick_ahrs;
madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);
interval_t interval;
interval_init(&interval);
float init = START_BETA;
udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);
/* kalman filter: */
kalman_t kalman1, kalman2, kalman3;
kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
vec3_t global_acc; /* x = N, y = E, z = D */
int init_done = 0;
int converged = 0;
sliding_avg_t *avg[3];
avg[0] = sliding_avg_create(1000, 0.0);
avg[1] = sliding_avg_create(1000, 0.0);
avg[2] = sliding_avg_create(1000, -9.81);
float alt_rel_last = 0.0;
int udp_cnt = 0;
while (1)
{
int i;
float dt = interval_measure(&interval);
init -= BETA_STEP;
if (init < FINAL_BETA)
{
init = FINAL_BETA;
init_done = 1;
}
madgwick_ahrs.beta = init;
/* sensor data acquisition: */
itg3200_read_gyro(&itg);
bma180_read_acc(&bma);
hmc5883_read(&hmc);
/* state estimates and output: */
euler_t euler;
madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
quat_t q_body_to_world;
quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
for (i = 0; i < 3; i++)
{
global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
}
if (init_done)
{
kalman_in_t kalman_in;
kalman_in.dt = dt;
kalman_in.pos = 0;
kalman_out_t kalman_out;
kalman_in.acc = global_acc.x;
kalman_run(&kalman_out, &kalman1, &kalman_in);
kalman_in.acc = global_acc.y;
kalman_run(&kalman_out, &kalman2, &kalman_in);
kalman_in.acc = -global_acc.z;
pthread_mutex_lock(&mutex);
kalman_in.pos = alt_rel;
pthread_mutex_unlock(&mutex);
kalman_run(&kalman_out, &kalman3, &kalman_in);
if (!converged)
{
if (fabs(kalman_out.pos - alt_rel) < 0.1)
{
converged = 1;
fprintf(stderr, "init done\n");
}
}
if (converged) // && udp_cnt++ > 10)
{
if (udp_cnt++ == 10)
{
char buffer[1024];
udp_cnt = 0;
int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
global_acc.x, global_acc.y, global_acc.z);
udp_socket_send(socket, buffer, len);
}
printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
fflush(stdout);
}
}
}
return 0;
}
/* return non zero if error */
static int udp_open(URLContext *h, const char *uri, int flags)
{
char hostname[1024], localaddr[1024] = "";
int port, udp_fd = -1, tmp, bind_ret = -1;
UDPContext *s = h->priv_data;
int is_output;
const char *p;
char buf[256];
struct sockaddr_storage my_addr;
socklen_t len;
int reuse_specified = 0;
int i, num_include_sources = 0, num_exclude_sources = 0;
char *include_sources[32], *exclude_sources[32];
h->is_streamed = 1;
h->max_packet_size = 1472;
is_output = !(flags & AVIO_FLAG_READ);
s->ttl = 16;
s->buffer_size = is_output ? UDP_TX_BUF_SIZE : UDP_MAX_PKT_SIZE;
p = strchr(uri, '?');
if (p) {
if (av_find_info_tag(buf, sizeof(buf), "reuse", p)) {
char *endptr = NULL;
s->reuse_socket = strtol(buf, &endptr, 10);
/* assume if no digits were found it is a request to enable it */
if (buf == endptr)
s->reuse_socket = 1;
reuse_specified = 1;
}
if (av_find_info_tag(buf, sizeof(buf), "ttl", p)) {
s->ttl = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "localport", p)) {
s->local_port = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "pkt_size", p)) {
h->max_packet_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "buffer_size", p)) {
s->buffer_size = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "connect", p)) {
s->is_connected = strtol(buf, NULL, 10);
}
if (av_find_info_tag(buf, sizeof(buf), "localaddr", p)) {
av_strlcpy(localaddr, buf, sizeof(localaddr));
}
if (av_find_info_tag(buf, sizeof(buf), "sources", p)) {
if (parse_source_list(buf, include_sources, &num_include_sources,
FF_ARRAY_ELEMS(include_sources)))
goto fail;
}
if (av_find_info_tag(buf, sizeof(buf), "block", p)) {
if (parse_source_list(buf, exclude_sources, &num_exclude_sources,
FF_ARRAY_ELEMS(exclude_sources)))
goto fail;
}
}
/* fill the dest addr */
av_url_split(NULL, 0, NULL, 0, hostname, sizeof(hostname), &port, NULL, 0, uri);
/* XXX: fix av_url_split */
if (hostname[0] == '\0' || hostname[0] == '?') {
/* only accepts null hostname if input */
if (!(flags & AVIO_FLAG_READ))
goto fail;
} else {
if (ff_udp_set_remote_url(h, uri) < 0)
goto fail;
}
if ((s->is_multicast || !s->local_port) && (h->flags & AVIO_FLAG_READ))
s->local_port = port;
udp_fd = udp_socket_create(s, &my_addr, &len, localaddr);
if (udp_fd < 0)
goto fail;
/* Follow the requested reuse option, unless it's multicast in which
* case enable reuse unless explicitly disabled.
*/
if (s->reuse_socket || (s->is_multicast && !reuse_specified)) {
s->reuse_socket = 1;
if (setsockopt (udp_fd, SOL_SOCKET, SO_REUSEADDR, &(s->reuse_socket), sizeof(s->reuse_socket)) != 0)
goto fail;
}
/* If multicast, try binding the multicast address first, to avoid
* receiving UDP packets from other sources aimed at the same UDP
* port. This fails on windows. This makes sending to the same address
* using sendto() fail, so only do it if we're opened in read-only mode. */
if (s->is_multicast && !(h->flags & AVIO_FLAG_WRITE)) {
bind_ret = bind(udp_fd,(struct sockaddr *)&s->dest_addr, len);
}
/* bind to the local address if not multicast or if the multicast
* bind failed */
/* the bind is needed to give a port to the socket now */
if (bind_ret < 0 && bind(udp_fd,(struct sockaddr *)&my_addr, len) < 0) {
log_net_error(h, AV_LOG_ERROR, "bind failed");
goto fail;
}
len = sizeof(my_addr);
getsockname(udp_fd, (struct sockaddr *)&my_addr, &len);
s->local_port = udp_port(&my_addr, len);
if (s->is_multicast) {
if (h->flags & AVIO_FLAG_WRITE) {
/* output */
if (udp_set_multicast_ttl(udp_fd, s->ttl, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
}
if (h->flags & AVIO_FLAG_READ) {
/* input */
if (num_include_sources && num_exclude_sources) {
av_log(h, AV_LOG_ERROR, "Simultaneously including and excluding multicast sources is not supported\n");
goto fail;
}
if (num_include_sources) {
if (udp_set_multicast_sources(udp_fd, (struct sockaddr *)&s->dest_addr, s->dest_addr_len, include_sources, num_include_sources, 1) < 0)
goto fail;
} else {
if (udp_join_multicast_group(udp_fd, (struct sockaddr *)&s->dest_addr) < 0)
goto fail;
}
if (num_exclude_sources) {
if (udp_set_multicast_sources(udp_fd, (struct sockaddr *)&s->dest_addr, s->dest_addr_len, exclude_sources, num_exclude_sources, 0) < 0)
goto fail;
}
}
}
if (is_output) {
/* limit the tx buf size to limit latency */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_SNDBUF, &tmp, sizeof(tmp)) < 0) {
log_net_error(h, AV_LOG_ERROR, "setsockopt(SO_SNDBUF)");
goto fail;
}
} else {
/* set udp recv buffer size to the largest possible udp packet size to
* avoid losing data on OSes that set this too low by default. */
tmp = s->buffer_size;
if (setsockopt(udp_fd, SOL_SOCKET, SO_RCVBUF, &tmp, sizeof(tmp)) < 0) {
log_net_error(h, AV_LOG_WARNING, "setsockopt(SO_RECVBUF)");
}
/* make the socket non-blocking */
ff_socket_nonblock(udp_fd, 1);
}
if (s->is_connected) {
if (connect(udp_fd, (struct sockaddr *) &s->dest_addr, s->dest_addr_len)) {
log_net_error(h, AV_LOG_ERROR, "connect");
goto fail;
}
}
for (i = 0; i < num_include_sources; i++)
av_freep(&include_sources[i]);
for (i = 0; i < num_exclude_sources; i++)
av_freep(&exclude_sources[i]);
s->udp_fd = udp_fd;
return 0;
fail:
if (udp_fd >= 0)
closesocket(udp_fd);
for (i = 0; i < num_include_sources; i++)
av_freep(&include_sources[i]);
for (i = 0; i < num_exclude_sources; i++)
av_freep(&exclude_sources[i]);
return AVERROR(EIO);
}
