int SensorState::update_sensor_state(uint64_t now)
{
ahrs_log(m_accel_last, m_accel, "accelerator");
ahrs_log(m_mag_last, m_mag, "magnetometer");
ahrs_log(m_gyro_last, m_gyro, "gyro");
bmp_log(m_temperature_last, m_temperature, "temperature");
bmp_log(m_pressure_last, m_pressure, "pressure");
// Clear stale data here. This is to prevent mahony_update()
// to accidentally reintegrate old data.
if (m_accel_last == m_accel.time)
m_accel.clear_data();
if (m_mag_last == m_mag.time)
m_mag.clear_data();
if (m_gyro_last == m_gyro.time)
m_gyro.clear_data();
if (mahony_update(m_state, m_gyro, m_accel, m_mag))
ahrs_log_state(now);
m_accel_last = m_accel.time;
m_gyro_last = m_gyro.time;
m_mag_last = m_mag.time;
m_temperature_last = m_temperature.time;
m_pressure_last = m_pressure.time;
return 0;
}
int
bmp_context_init(int context)
{
avl_tree *tree;
int af, rc;
/*
* Initialize the BMP tables
*/
for (af = 0; af < BGP_AF_MAX; af++) {
rc = bmp_table_init(af);
if (rc < 0 ) {
bmp_log("bmp table init failed for af %d", af);
return -1;
}
}
/*
* Initialize the BMP routers
*/
tree = avl_init(bgp_router_compare, NULL, AVL_TREE_INTRUSIVE);
if (tree == NULL) {
bmp_log("failed to initialize routers tree");
return -1;
}
bmp_contexts[context].bgp_routers = tree;
return 0;
}
/*
* accept() connections from a server socket fd and create new sessions
*/
static int
bmp_accept_sessions(bmp_server *server, int events)
{
int fd, rc;
struct sockaddr caddr;
socklen_t slen;
while (1) {
slen = sizeof(caddr);
fd = accept(server->listen, &caddr, &slen);
if (fd < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK) break;
bmp_log("accept() failed: %s", strerror(errno));
} else {
rc = bmp_session_create(server, fd, &caddr, slen);
}
}
return rc;
}
int
bmp_server_init(int port, int interactive)
{
int rc = 0, timer;
struct epoll_event ev;
struct sockaddr_in saddr;
signal(SIGPIPE, SIG_IGN);
signal(SIGINT, bmp_server_exit);
signal(SIGTERM, bmp_server_exit);
memset(&server, 0, sizeof(bmp_server));
server.pid = getpid();
server.port = port;
memset(&saddr, 0, sizeof(saddr));
saddr.sin_family = AF_INET;
saddr.sin_addr.s_addr = INADDR_ANY;
saddr.sin_port = htons(server.port);
server.listen = socket(AF_INET, SOCK_STREAM, 0);
if (server.listen < 0) {
bmp_log("socket() failed: %s", strerror(errno));
exit(1);
}
rc = so_reuseaddr(server.listen);
if (rc < 0) {
return rc;
}
rc = bind(server.listen, (struct sockaddr *) &saddr, sizeof(saddr));
if (rc < 0) {
bmp_log("bind() failed: %s", strerror(errno));
return rc;
}
rc = listen(server.listen, BMP_SESSION_MAX);
if (rc < 0) {
bmp_log("listen() failed: %s", strerror(errno));
return rc;
}
rc = fd_nonblock(server.listen);
if (rc < 0) {
return rc;
}
/*
* Create the epoll instance and register the listen socket with the server
* epoll queue
*/
server.eq = epoll_create(BMP_SESSION_MAX);
if (server.eq < 0) {
bmp_log("epoll_create1() failed: %s", strerror(errno));
return -1;
}
MONITOR_FD(server.eq, server.listen, rc);
if (rc < 0) {
bmp_log("epoll_ctl(server->fd) failed: %s", strerror(errno));
return rc;
}
/*
* Create a timer fd and register the read-end of the timer pipe with the
* server's epoll queue
*/
timer = bmp_timer_init();
if (timer < 0) {
bmp_log("Timer init failed");
return -1;
}
server.timer = timer;
MONITOR_FD(server.eq, timer, rc);
if (rc < 0) {
bmp_log("server timer listen error: %s", timer, strerror(errno));
return rc;
}
/*
* Allocate the clients event queue
*/
server.ev = calloc(BMP_SESSION_MAX, sizeof(ev));
if (server.ev == NULL) {
bmp_log("calloc(server->ev) failed");
return -1;
}
/*
* Initialize the session AVL tree
*/
server.sessions = avl_init(bmp_session_compare, NULL, AVL_TREE_INTRUSIVE);
/*
* Start the command processing task
*/
rc = bmp_command_init(interactive);
if (rc < 0) {
return -1;
}
/*
* Initialize the main BMP "processing" task
*/
rc = bmp_process_init();
if (rc < 0) {
return -1;
}
/*
* Initialize the realtime "online" BMP context
*/
rc = bmp_context_init(0);
if (rc < 0) {
return -1;
}
return rc;
}
int
bmp_server_run()
{
int i, ev, n, fd, rc;
gettimeofday(&server.time, 0);
rc = bmp_command_run();
if (rc < 0) {
return -1;
}
rc = bmp_process_run();
if (rc < 0) {
return -1;
}
while (1) {
/*
* Main blocking call
*/
n = epoll_wait(server.eq, server.ev, BMP_SESSION_MAX, -1);
if (n < 0) {
if (errno == EINTR) {
continue;
} else {
bmp_log("epoll_wait error: %s", strerror(errno));
return -1;
}
}
for (i = 0; i < n; i++) {
ev = server.ev[i].events;
fd = server.ev[i].data.fd;
if ((ev & EPOLLERR) || (ev & EPOLLHUP)) {
/*
* Error
*/
continue;
}
if (fd == server.listen) { // listen socket - accept sessions
bmp_accept_sessions(&server, ev);
} else if (fd == server.timer) { // periodic timer
bmp_server_timer_process(&server, fd);
} else { // session events
bmp_session_process(&server, fd, ev);
}
}
}
close(server.listen);
close(server.eq);
free(server.ev);
return 0;
}
