From da838251c5d7418a7a4f28852825c0134dc6ba8b Mon Sep 17 00:00:00 2001 From: rockkingjy Date: Thu, 8 Nov 2018 15:57:02 +0900 Subject: [PATCH 1/2] debug opentracker --- .gitignore | 1 + README.md | 6 +++--- src/Config/Module.h | 5 +++++ src/Config/switch.h | 8 ++++---- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index c1fdf4752..f8044519c 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ /Debug/ /Release/ /build/ +.vscode/ *.prefs .project .cproject diff --git a/README.md b/README.md index 38c6b1ea2..c868e6178 100644 --- a/README.md +++ b/README.md @@ -27,9 +27,9 @@ External camera (Click to jump to YouTube): ## Build & Run -###[Build on NVIDIA Jetson TX1](https://github.com/yankailab/OpenKAI/blob/master/doc/JetsonTX1/build.md) +### [Build on NVIDIA Jetson TX1](https://github.com/yankailab/OpenKAI/blob/master/doc/JetsonTX1/build.md) -###[Build on Ubuntu desktop systems](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/build.md) +### [Build on Ubuntu desktop systems](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/build.md) ## Application implementation There are two ways of implement an application based on OpenKAI: compilation vs. script. @@ -37,7 +37,7 @@ There are two ways of implement an application based on OpenKAI: compilation vs. ### Compilation OpenKAI is implemented purely in C++, it is very easy to implement your application as C/C++ and compile with OpenKAI together. Refer to "OpenKAI/src/Application/Startup.cpp" as a starting point for application integration, and this guide: -###[Develop OpenKAI apps with Eclipse CDT](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/eclipse.md) +### [Develop OpenKAI apps with Eclipse CDT](https://github.com/yankailab/OpenKAI/blob/master/doc/x86_64/Ubuntu/eclipse.md) ### Kiss (OpenKai Simple Script) If the existing function Modules from OpenKAI already meet your needs, you can write a json-like Kiss script to define your application. Note that Kiss is treated as a config file and will be parsed all at once at the beginning of OpenKAI program execution. The Modules defined by Kiss file are statically created and get started to run. Therefore there is no difference in execution speed between the Compilation way and application defined by Kiss (To be updated). diff --git a/src/Config/Module.h b/src/Config/Module.h index 0d815d951..5370700db 100644 --- a/src/Config/Module.h +++ b/src/Config/Module.h @@ -42,9 +42,14 @@ #include "../SLAM/_ORB_SLAM2.h" #include "../SLAM/_LidarSlam.h" +#ifdef USE_OPENCV_CONTRIB #include "../Tracker/_SingleTracker.h" #include "../Tracker/_MultiTracker.h" +#endif + +#ifdef USE_OPENTRACKER #include "../Tracker/_EcoTracker.h" +#endif #include "../UI/Window.h" diff --git a/src/Config/switch.h b/src/Config/switch.h index 0f502e052..9ae8b9428 100644 --- a/src/Config/switch.h +++ b/src/Config/switch.h @@ -1,8 +1,8 @@ -#define USE_CUDA -#define USE_OPENCV_CONTRIB -#define USE_DARKNET -#define USE_OPENTRACKER +//#define USE_CUDA +//#define USE_OPENCV_CONTRIB +//#define USE_DARKNET +//#define USE_OPENTRACKER //#define USE_REALSENSE //#define USE_TENSORFLOW //#define USE_PYLON From 242aa35c9728063f1e737011185d6ec797c8290a Mon Sep 17 00:00:00 2001 From: rockkingjy Date: Thu, 6 Dec 2018 18:28:13 +0900 Subject: [PATCH 2/2] add LidarOuster driver in src/Sensor/ --- kiss/test/_LeddarVu.kiss | 91 +---------- kiss/test/_LidarOuster.kiss | 32 ++++ src/Config/Module.cpp | 1 + src/Config/Module.h | 1 + src/Sensor/LidarOuster/_LidarOuster.cpp | 136 ++++++++++++++++ src/Sensor/LidarOuster/_LidarOuster.h | 66 ++++++++ src/Sensor/LidarOuster/os1.cpp | 198 ++++++++++++++++++++++++ src/Sensor/LidarOuster/os1.h | 62 ++++++++ src/Sensor/LidarOuster/os1_packet.h | 189 ++++++++++++++++++++++ 9 files changed, 688 insertions(+), 88 deletions(-) create mode 100644 kiss/test/_LidarOuster.kiss create mode 100644 src/Sensor/LidarOuster/_LidarOuster.cpp create mode 100644 src/Sensor/LidarOuster/_LidarOuster.h create mode 100644 src/Sensor/LidarOuster/os1.cpp create mode 100644 src/Sensor/LidarOuster/os1.h create mode 100644 src/Sensor/LidarOuster/os1_packet.h diff --git a/kiss/test/_LeddarVu.kiss b/kiss/test/_LeddarVu.kiss index b6a6b224c..933ee109a 100644 --- a/kiss/test/_LeddarVu.kiss +++ b/kiss/test/_LeddarVu.kiss @@ -8,31 +8,18 @@ "waitKey":30, "presetDir":"/home/ubuntu/src/OpenKAI/data/", } - +/* { "name":"OKview", "class":"Window", "bInst":1, - "bFullScreen":1, + "bFullScreen":0, "bRec":0, "recFile":"/media/ubuntu/data/OpenKAI_", "recFPS":15, "recCodec":"MJPG", } - -{ - "name":"camera", - "class":"_Camera", - "FPS":30, - "bInst":0, - "Window":"OKview", - "deviceID":1, - "width":1920, - "height":1080, - "fovV":60, - "fovH":90, -} - +*/ { "name":"leddarVu0", "class":"_LeddarVu", @@ -68,75 +55,3 @@ "nAvr":3, "nMed":3, } - -{ - "name":"leddarVu1", - "class":"_LeddarVu", - "FPS":30, - "bInst":1, - "bLog":1, - "Window":"OKview", - "rMin":0.1, - "rMax":100.0, - "fovH":48, - "fovV":0.3, - "nDiv":8, - "showScale":30.0, - "showDegOffset":-24.0, - "showOriginOffsetX":0.5, - "showOriginOffsetY":1.0, - "_VisionBase":"camera", - "portName":"/dev/ttyACM1", - "baud":115200, - "bUse0x41":0, - "slaveAddr":1, - "nAccumulationsExpo":5, - "nOversamplingsExpo":1, - "nPoint":20, - "lightSrcPwr":100, - "bAutoLightSrcPwr":0, - "bDemergeObj":1, - "bStaticNoiseRemoval":1, - "bPrecision":1, - "bSaturationCompensation":1, - "bOvershootManagement":1, - "oprMode":1, - "nAvr":3, - "nMed":3, -} - -{ - "name":"leddarVu2", - "class":"_LeddarVu", - "FPS":30, - "bInst":1, - "bLog":1, - "Window":"OKview", - "rMin":0.1, - "rMax":100.0, - "fovH":48, - "fovV":0.3, - "nDiv":8, - "showScale":30.0, - "showDegOffset":-24.0, - "showOriginOffsetX":0.5, - "showOriginOffsetY":1.0, - "_VisionBase":"camera", - "portName":"/dev/ttyACM2", - "baud":115200, - "bUse0x41":0, - "slaveAddr":1, - "nAccumulationsExpo":5, - "nOversamplingsExpo":1, - "nPoint":20, - "lightSrcPwr":100, - "bAutoLightSrcPwr":0, - "bDemergeObj":1, - "bStaticNoiseRemoval":1, - "bPrecision":1, - "bSaturationCompensation":1, - "bOvershootManagement":1, - "oprMode":1, - "nAvr":3, - "nMed":3, -} diff --git a/kiss/test/_LidarOuster.kiss b/kiss/test/_LidarOuster.kiss new file mode 100644 index 000000000..60a7f04d3 --- /dev/null +++ b/kiss/test/_LidarOuster.kiss @@ -0,0 +1,32 @@ +{ + "name":"APP", + "class":"Startup", + "appName":"OpenKAI", + "bWindow":0, + "bDraw":0, + "bLog":0, + "waitKey":30, + "presetDir":"/home/ubuntu/src/OpenKAI/data/", +} +/* +{ + "name":"OKview", + "class":"Window", + "bInst":1, + "bFullScreen":0, + "bRec":0, + "recFile":"/media/ubuntu/data/OpenKAI_", + "recFPS":15, + "recCodec":"MJPG", +} +*/ +{ + "name":"LidarOuster", + "class":"_LidarOuster", + "FPS":10000, + "bInst":1, + "bLog":1, + "Window":"OKview", + "hostname":"192.168.100.70", + "udp_dest_host": "192.168.100.95", +} diff --git a/src/Config/Module.cpp b/src/Config/Module.cpp index c9dab9c94..a741ed5d2 100644 --- a/src/Config/Module.cpp +++ b/src/Config/Module.cpp @@ -32,6 +32,7 @@ BASE* Module::createInstance(Kiss* pK) ADD_MODULE(_Lane); ADD_MODULE(_LidarSlam); ADD_MODULE(_LeddarVu); + ADD_MODULE(_LidarOuster); ADD_MODULE(_Mavlink); ADD_MODULE(_Path); ADD_MODULE(PIDctrl); diff --git a/src/Config/Module.h b/src/Config/Module.h index a6c1b5425..324e5c5ee 100644 --- a/src/Config/Module.h +++ b/src/Config/Module.h @@ -37,6 +37,7 @@ #include "../Protocol/_Mavlink.h" #include "../Protocol/_RC.h" +#include "../Sensor/LidarOuster/_LidarOuster.h" #include "../Sensor/_LeddarVu.h" #include "../Sensor/RPLIDAR/_RPLIDAR.h" #include "../SLAM/_ORB_SLAM2.h" diff --git a/src/Sensor/LidarOuster/_LidarOuster.cpp b/src/Sensor/LidarOuster/_LidarOuster.cpp new file mode 100644 index 000000000..ff89fd81b --- /dev/null +++ b/src/Sensor/LidarOuster/_LidarOuster.cpp @@ -0,0 +1,136 @@ +/* + * Created on: Nov 24, 2017 + * Author: yankai + */ +#include "_LidarOuster.h" + +namespace kai +{ + +_LidarOuster::_LidarOuster() +{ + m_hostname = ""; + m_udp_dest_host = ""; +} + +_LidarOuster::~_LidarOuster() +{ + m_bThreadON = false; +} + +void _LidarOuster::handle_lidar(uint8_t *buf) +{ + m_n_lidar_packets++; + m_lidar_col_0_ts = col_timestamp(nth_col(0, buf)); + m_lidar_col_0_h_angle = col_h_angle(nth_col(0, buf)); +} + +void _LidarOuster::handle_imu(uint8_t *buf) +{ + m_n_imu_packets++; + m_imu_ts = imu_sys_ts(buf); + m_imu_av_z = imu_av_z(buf); + m_imu_la_y = imu_la_y(buf); +} + +bool _LidarOuster::init(void *pKiss) +{ + IF_F(!this->_ThreadBase::init(pKiss)); + Kiss *pK = (Kiss *)pKiss; + + KISSm(pK, hostname); + KISSm(pK, udp_dest_host); + return true; +} + +bool _LidarOuster::start(void) +{ + m_bThreadON = true; + int retCode = pthread_create(&m_threadID, 0, getUpdateThread, this); + if (retCode != 0) + { + LOG_E(retCode); + m_bThreadON = false; + return false; + } + + return true; +} + +void _LidarOuster::update(void) +{ + while (m_bThreadON) + { + if (!m_flag_open) + { + m_flag_open = open(); + if (!m_flag_open) + { + this->sleepTime(USEC_1SEC); + continue; + } + } + + this->autoFPSfrom(); + updateLidar(); + this->autoFPSto(); + } +} + +bool _LidarOuster::open(void) +{ + m_cli = init_client(m_hostname, m_udp_dest_host, 7502, 7503); + if (!m_cli) + { + std::cerr << "Failed to connect to : " << m_hostname << std::endl; + return false; + } + return true; +} + +void _LidarOuster::updateLidar(void) +{ + client_state st = poll_client(*m_cli); + if (st & ERROR) + { + std::cerr << "Fail to update Lidar" << std::endl; + } + else if (st & LIDAR_DATA) + { + if (read_lidar_packet(*m_cli, m_lidar_buf)) + handle_lidar(m_lidar_buf); + } + else if (st & IMU_DATA) + { + if (read_imu_packet(*m_cli, m_imu_buf)) + handle_imu(m_imu_buf); + } +} + +bool _LidarOuster::console(int &iY) +{ + IF_F(!this->_ThreadBase::console(iY)); + + string msg; + msg += "n_lidar_packets=" + li2str(m_n_lidar_packets); + msg += "; col_0_azimuth=" + f2str(m_lidar_col_0_h_angle, 6); + msg += "; col_0_ts=" + li2str(m_lidar_col_0_ts); + + COL_MSG; + iY++; + mvaddstr(iY, CONSOLE_X_MSG, msg.c_str()); + + msg = ""; + msg += "n_imu_packets=" + li2str(m_n_imu_packets); + msg += "; im_av_z=" + f2str(m_imu_av_z, 6); + msg += "; im_la_y=" + f2str(m_imu_la_y, 6); + msg += "; imu_ts=" + li2str(m_imu_ts); + + COL_MSG; + iY++; + mvaddstr(iY, CONSOLE_X_MSG, msg.c_str()); + + return true; +} + +} // namespace kai diff --git a/src/Sensor/LidarOuster/_LidarOuster.h b/src/Sensor/LidarOuster/_LidarOuster.h new file mode 100644 index 000000000..d58cea963 --- /dev/null +++ b/src/Sensor/LidarOuster/_LidarOuster.h @@ -0,0 +1,66 @@ +/* + * _RPLIDAR.h + * + * Created on: Apr 3, 2017 + * Author: yankai + */ + +#ifndef OpenKAI_src_Sensor__LidarOuster_H_ +#define OpenKAI_src_Sensor__LidarOuster_H_ + +#include "../../Base/_ThreadBase.h" +#include "os1.h" +#include "os1_packet.h" + +namespace kai +{ +using namespace ouster::OS1; + +class _LidarOuster : public _ThreadBase +{ + public: + _LidarOuster(); + ~_LidarOuster(); + + bool init(void *pKiss); + bool start(void); + //bool draw(void); + bool console(int &iY); + + private: + bool open(void); + void update(void); + void updateLidar(void); + static void *getUpdateThread(void *This) + { + ((_LidarOuster *)This)->update(); + return NULL; + } + + void handle_lidar(uint8_t *buf); + void handle_imu(uint8_t *buf); + + public: + // To get the ip address of lidar, run in shell: + // nmap --open -p T:7501 192.168.100.0/24 + string m_hostname; // ip address of the lidar; + string m_udp_dest_host; // ip address of the host; + std::shared_ptr m_cli; + bool m_flag_open = false; + + // Lidar; + uint64_t m_n_lidar_packets = 0; // package number; + uint64_t m_lidar_col_0_ts = 0; // timestamp; + float m_lidar_col_0_h_angle = 0.0; // angle for the col 0; + uint8_t m_lidar_buf[lidar_packet_bytes + 1]; + + // IMU + uint64_t m_n_imu_packets = 0; // package number; + uint64_t m_imu_ts = 0; // timestamp; + float m_imu_av_z = 0.0; // angular velocity z; + float m_imu_la_y = 0.0; // linear acceleration y; + uint8_t m_imu_buf[imu_packet_bytes + 1]; +}; + +} // namespace kai +#endif diff --git a/src/Sensor/LidarOuster/os1.cpp b/src/Sensor/LidarOuster/os1.cpp new file mode 100644 index 000000000..3c7deeb5a --- /dev/null +++ b/src/Sensor/LidarOuster/os1.cpp @@ -0,0 +1,198 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "os1.h" + +namespace ouster { +namespace OS1 { + +using ns = std::chrono::nanoseconds; + +struct client { + int lidar_fd; + int imu_fd; + ~client() { + close(lidar_fd); + close(imu_fd); + } +}; + +static int udp_data_socket(int port) { + int sock_fd = socket(PF_INET, SOCK_DGRAM, 0); + if (sock_fd < 0) { + std::cerr << "socket: " << std::strerror(errno) << std::endl; + return -1; + } + + struct sockaddr_in my_addr; + memset((char*)&my_addr, 0, sizeof(my_addr)); + my_addr.sin_family = AF_INET; + my_addr.sin_port = htons(port); + my_addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(sock_fd, (struct sockaddr*)&my_addr, sizeof(my_addr)) < 0) { + std::cerr << "bind: " << std::strerror(errno) << std::endl; + return -1; + } + + struct timeval timeout; + timeout.tv_sec = 1; + timeout.tv_usec = 0; + if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, + sizeof(timeout)) < 0) { + std::cerr << "setsockopt: " << std::strerror(errno) << std::endl; + return -1; + } + + return sock_fd; +} + +static int cfg_socket(const char* addr) { + struct addrinfo hints, *info_start, *ai; + + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + + int ret = getaddrinfo(addr, "7501", &hints, &info_start); + if (ret != 0) { + std::cerr << "getaddrinfo: " << gai_strerror(ret) << std::endl; + return -1; + } + if (info_start == NULL) { + std::cerr << "getaddrinfo: empty result" << std::endl; + return -1; + } + + int sock_fd; + for (ai = info_start; ai != NULL; ai = ai->ai_next) { + sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); + if (sock_fd < 0) { + std::cerr << "socket: " << std::strerror(errno) << std::endl; + continue; + } + + if (connect(sock_fd, ai->ai_addr, ai->ai_addrlen) == -1) { + close(sock_fd); + continue; + } + + break; + } + + freeaddrinfo(info_start); + if (ai == NULL) { + return -1; + } + + return sock_fd; +} + +std::shared_ptr init_client(const std::string& hostname, + const std::string& udp_dest_host, + int lidar_port, int imu_port) { + int sock_fd = cfg_socket(hostname.c_str()); + + if (sock_fd < 0) return std::shared_ptr(); + + std::string cmd; + + auto do_cmd = [&](const std::string& op, const std::string& val) { + const size_t max_res_len = 64; + char read_buf[max_res_len + 1]; + + ssize_t len; + std::string cmd = op + " " + val + "\n"; + + len = write(sock_fd, cmd.c_str(), cmd.length()); + if (len != (ssize_t)cmd.length()) { + std::cerr << "init_client: failed to send command" << std::endl; + return false; + } + + len = read(sock_fd, read_buf, max_res_len); + if (len < 0) { + std::cerr << "read: " << std::strerror(errno) << std::endl; + return false; + } + read_buf[len] = '\0'; + + auto res = std::string(read_buf); + res.erase(res.find_last_not_of(" \r\n\t") + 1); + + if (res != op) { + std::cerr << "init_client: command \"" << cmd << "\" failed with \"" + << res << "\"" << std::endl; + return false; + } + return true; + }; + + bool success = true; + success &= do_cmd("set_udp_port_lidar", std::to_string(lidar_port)); + success &= do_cmd("set_udp_port_imu", std::to_string(imu_port)); + success &= do_cmd("set_udp_ip", udp_dest_host); + + if (!success) return std::shared_ptr(); + + close(sock_fd); + + int lidar_fd = udp_data_socket(lidar_port); + int imu_fd = udp_data_socket(imu_port); + auto cli = std::make_shared(); + cli->lidar_fd = lidar_fd; + cli->imu_fd = imu_fd; + return cli; +} + +client_state poll_client(const client& c) { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(c.lidar_fd, &rfds); + FD_SET(c.imu_fd, &rfds); + + int max_fd = std::max(c.lidar_fd, c.imu_fd); + + int retval = select(max_fd + 1, &rfds, NULL, NULL, NULL); + + client_state res = client_state(0); + if (retval == -1) { + std::cerr << "select: " << std::strerror(errno) << std::endl; + res = client_state(res | ERROR); + } else if (retval) { + if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA); + if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | IMU_DATA); + } + return res; +} + +static bool recv_fixed(int fd, void* buf, size_t len) { + ssize_t n = recvfrom(fd, buf, len + 1, 0, NULL, NULL); + if (n == (ssize_t)len) + return true; + else if (n == -1) + std::cerr << "recvfrom: " << std::strerror(errno) << std::endl; + else + std::cerr << "Unexpected udp packet length: " << n << std::endl; + return false; +} + +bool read_lidar_packet(const client& cli, uint8_t* buf) { + return recv_fixed(cli.lidar_fd, buf, lidar_packet_bytes); +} + +bool read_imu_packet(const client& cli, uint8_t* buf) { + return recv_fixed(cli.imu_fd, buf, imu_packet_bytes); +} +} +} diff --git a/src/Sensor/LidarOuster/os1.h b/src/Sensor/LidarOuster/os1.h new file mode 100644 index 000000000..f2cd36fcf --- /dev/null +++ b/src/Sensor/LidarOuster/os1.h @@ -0,0 +1,62 @@ +/** + * OS1 sample client + */ + +#pragma once + +#include +#include +#include + +namespace ouster { +namespace OS1 { + +const size_t lidar_packet_bytes = 12608; +const size_t imu_packet_bytes = 48; + +struct client; + +enum client_state { ERROR = 1, LIDAR_DATA = 2, IMU_DATA = 4 }; + +/** + * Connect to the sensor and start listening for data + * @param hostname hostname or ip of the sensor + * @param udp_dest_host hostname or ip where the sensor should send data + * @param lidar_port port on which the sensor will send lidar data + * @param imu_port port on which the sensor will send imu data + * @return pointer owning the resources associated with the connection + */ +std::shared_ptr init_client(const std::string& hostname, + const std::string& udp_dest_host, + int lidar_port, int imu_port); + +/** + * Block for up to a second until either data is ready or an error occurs. + * @param cli client returned by init_client associated with the connection + * @return client_state s where (s & ERROR) is true if an error occured, (s & + * LIDAR_DATA) is true if lidar data is ready to read, and (s & IMU_DATA) is + * true if imu data is ready to read + */ +client_state poll_client(const client& cli); + +/** + * Read lidar data from the sensor. Will block for up to a second if no data is + * available. + * @param cli client returned by init_client associated with the connection + * @param buf buffer to which to write lidar data. Must be at least + * lidar_packet_bytes + 1 bytes + * @returns true if a lidar packet was successfully read + */ +bool read_lidar_packet(const client& cli, uint8_t* buf); + +/** + * Read imu data from the sensor. Will block for up to a second if no data is + * available. + * @param cli client returned by init_client associated with the connection + * @param buf buffer to which to write imu data. Must be at least + * imu_packet_bytes + 1 bytes + * @returns true if an imu packet was successfully read + */ +bool read_imu_packet(const client& cli, uint8_t* buf); +} +} diff --git a/src/Sensor/LidarOuster/os1_packet.h b/src/Sensor/LidarOuster/os1_packet.h new file mode 100644 index 000000000..6f8594357 --- /dev/null +++ b/src/Sensor/LidarOuster/os1_packet.h @@ -0,0 +1,189 @@ +/** + * Utilities to parse lidar and imu packets + */ + +#pragma once + +#include +#include +#include +#include + +namespace ouster { +namespace OS1 { + +namespace { + +const int pixels_per_column = 64; +const int columns_per_buffer = 16; + +const int pixel_bytes = 12; +const int column_bytes = 16 + (pixels_per_column * pixel_bytes) + 4; + +const int encoder_ticks_per_rev = 90112; + +const std::array beam_altitude_angles = { + 16.611, 16.084, 15.557, 15.029, 14.502, 13.975, 13.447, 12.920, + 12.393, 11.865, 11.338, 10.811, 10.283, 9.756, 9.229, 8.701, + 8.174, 7.646, 7.119, 6.592, 6.064, 5.537, 5.010, 4.482, + 3.955, 3.428, 2.900, 2.373, 1.846, 1.318, 0.791, 0.264, + -0.264, -0.791, -1.318, -1.846, -2.373, -2.900, -3.428, -3.955, + -4.482, -5.010, -5.537, -6.064, -6.592, -7.119, -7.646, -8.174, + -8.701, -9.229, -9.756, -10.283, -10.811, -11.338, -11.865, -12.393, + -12.920, -13.447, -13.975, -14.502, -15.029, -15.557, -16.084, -16.611, +}; +const std::array beam_azimuth_angles = { + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, + 3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164, +}; + +struct trig_table_entry { + float sin_beam_altitude_angles; + float cos_beam_altitude_angles; + float beam_azimuth_angles; +}; + +// table of vertical angle cos, sin, and horizontal offset of each pixel +static trig_table_entry trig_table[pixels_per_column]; +} + +static bool init_tables() { + for (int i = 0; i < pixels_per_column; i++) { + trig_table[i] = {sinf(beam_altitude_angles[i] * 2 * M_PI / 360.0f), + cosf(beam_altitude_angles[i] * 2 * M_PI / 360.0f), + beam_azimuth_angles[i] * 2 * (float)M_PI / 360.0f}; + } + return true; +} + +static bool tables_initialized = init_tables(); + +// lidar column fields +inline const uint8_t* nth_col(int n, const uint8_t* udp_buf) { + return udp_buf + (n * column_bytes); +} + +inline uint64_t col_timestamp(const uint8_t* col_buf) { + uint64_t res; + memcpy(&res, col_buf, sizeof(uint64_t)); + return res; +} + +inline float col_h_angle(const uint8_t* col_buf) { + uint32_t ticks; + memcpy(&ticks, col_buf + (3 * 4), sizeof(uint32_t)); + return (2.0 * M_PI * ticks / (float)encoder_ticks_per_rev); +} + +inline float col_h_encoder_count(const uint8_t* col_buf) { + uint32_t res; + std::memcpy(&res, col_buf + (3 * 4), sizeof(uint32_t)); + return res; +} + +inline uint32_t col_measurement_id(const uint8_t* col_buf) { + uint32_t res; + memcpy(&res, col_buf + (2 * 4), sizeof(uint32_t)); + return res; +} + +inline uint32_t col_valid(const uint8_t* col_buf) { + uint32_t res; + memcpy(&res, col_buf + (196 * 4), sizeof(uint32_t)); + return res; +} + +// lidar pixel fields +inline const uint8_t* nth_px(int n, const uint8_t* col_buf) { + return col_buf + 16 + (n * pixel_bytes); +} + +inline uint32_t px_range(const uint8_t* px_buf) { + uint32_t res; + memcpy(&res, px_buf, sizeof(uint32_t)); + res &= 0x000fffff; + return res; +} + +inline uint16_t px_reflectivity(const uint8_t* px_buf) { + uint16_t res; + memcpy(&res, px_buf + 4, sizeof(uint16_t)); + return res; +} + +inline uint16_t px_signal_photons(const uint8_t* px_buf) { + uint16_t res; + memcpy(&res, px_buf + 6, sizeof(uint16_t)); + return res; +} + +inline uint16_t px_noise_photons(const uint8_t* px_buf) { + uint16_t res; + memcpy(&res, px_buf + 8, sizeof(uint16_t)); + return res; +} + +// imu packets +inline uint64_t imu_sys_ts(const uint8_t* imu_buf) { + uint64_t res; + memcpy(&res, imu_buf, sizeof(uint64_t)); + return res; +} + +inline uint64_t imu_accel_ts(const uint8_t* imu_buf) { + uint64_t res; + memcpy(&res, imu_buf + 8, sizeof(uint64_t)); + return res; +} + +inline uint64_t imu_gyro_ts(const uint8_t* imu_buf) { + uint64_t res; + memcpy(&res, imu_buf + 16, sizeof(uint64_t)); + return res; +} + +// imu linear acceleration +inline float imu_la_x(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 24, sizeof(float)); + return res; +} + +inline float imu_la_y(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 28, sizeof(float)); + return res; +} + +inline float imu_la_z(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 32, sizeof(float)); + return res; +} + +// imu angular velocity +inline float imu_av_x(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 36, sizeof(float)); + return res; +} + +inline float imu_av_y(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 40, sizeof(float)); + return res; +} + +inline float imu_av_z(const uint8_t* imu_buf) { + float res; + memcpy(&res, imu_buf + 44, sizeof(float)); + return res; +} +} +}