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