From b0eb0652f9454f7799757855c072ec26ac794dc7 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Wed, 18 Jun 2025 01:01:44 +0200 Subject: [PATCH] GNSS reciever now also using NMEA GNRMC. ALso lots of small changes everywhere --- CMakeLists.txt | 5 + README.md | 7 +- cmake_build.sh | 2 +- processes/GNSS_reader/gnssrtk.cpp | 292 ++++++++++++++++++------ processes/GNSS_reader/include/gnssrtk.h | 38 +-- processes/GNSS_reader/main.cpp | 11 +- processes/click_reader/main.cpp | 6 +- processes/controller/main.cpp | 2 +- processes/include/system_def.h | 5 + processes/include/util.h | 64 +++++- processes/udp_server/main.cpp | 4 +- processes/udp_server/udp_server.cpp | 2 +- 12 files changed, 316 insertions(+), 122 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee6a1dd..e10f23c 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,11 @@ SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${GCC_COVERAGE_LINK_FLAGS set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) +# add_definitions(-DLOG_DEBUG) +add_definitions(-DLOG_INFO) +# add_definitions(-DLOG_WARN) +#add_definitions(-DLOG_ERR) + find_library(BCM2835_LIB bcm2835) add_subdirectory(lib/mavlink) add_subdirectory(lib/spdlog) diff --git a/README.md b/README.md index 0cfbd3d..ff3f365 100755 --- a/README.md +++ b/README.md @@ -27,8 +27,7 @@ just run wvdial, correct settings should be in `/etc/wvdial.conf`. - EKF for IMU (maybe incorporate GNSS, probably not). - Yaw, maybe add time-derivates to attitude? - Fix all unhandled mavlink messages. - - GNSS. add GPS raw data with number of satelittes etc etc. - - add support for more NMEA commands, especially $GNRMC + - GNSS, see TODO comments + - Fix cpp code (See GNSS for reference, remove header inclusion by using forward declaration, remove stuff from headers etc). - Start working on xbox controller, PWM, ESC, etc. - - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled? - + - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled? \ No newline at end of file diff --git a/cmake_build.sh b/cmake_build.sh index 52d0e72..39f500e 100755 --- a/cmake_build.sh +++ b/cmake_build.sh @@ -1,4 +1,4 @@ cd $(dirname "$0")/build -cmake -G Ninja .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j2 +cmake -G Ninja .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ninja -j2 \ No newline at end of file diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp index 92046bb..59bea9e 100644 --- a/processes/GNSS_reader/gnssrtk.cpp +++ b/processes/GNSS_reader/gnssrtk.cpp @@ -4,20 +4,63 @@ #include #include #include + +#include + #include +#include -#include "util.h" #include "system_def.h" +static const char GNSSRTK_RSP_INIT ='$'; +static const char GNSSRTK_RSP_DELIMITER = ','; +static const std::string GNSSRTK_RSP_GNGGA = "$GNGGA"; +static const std::string GNSSRTK_RSP_GNRMC = "$GNRMC"; + +static const int GNSSRTK_DRV_TX_BUFFER_SIZE = 100; +static const int GNSSRTK_DRV_RX_BUFFER_SIZE = 300; + static int serial_fd; static std::shared_ptr logger; -err_t init_uart(std::shared_ptr logg) +typedef struct { + std::string time; + std::string latitude; + std::string latitude_hem; + std::string longitude; + std::string longitude_hem; + std::string status; // 0 = no fix, 1 = GPS fix, 2 = DGPS fix, 3 = PPS fix, 4 = RTK fixed, 5 = RTK float + std::string number_of_sattelites; + std::string h_dilution_of_pos; + std::string altitude; +} gnssrtk_gngga_t; + +typedef struct { + std::string time; + std::string status; + std::string latitude; + std::string latitude_hem; + std::string longitude; + std::string longitude_hem; + std::string ground_speed; + std::string ground_heading; + std::string date; + std::string magnetic_declination; + std::string magnetic_declination_direction; + std::string mode; +} gnssrtk_gnrmc_t; + +static AtomicWrap latest_gngga; +static AtomicWrap latest_gnrmc; + +err_t init_uart(std::shared_ptr logg, + const std::string& device, + unsigned int baud_rate) { logger = logg; struct termios options; - serial_fd = open(SERIAL_DEVICE, O_RDONLY | O_NOCTTY); + serial_fd = open(device.c_str(), O_RDONLY | O_NOCTTY); if (serial_fd == -1) { logger->error("Failed to open serial port."); @@ -39,7 +82,7 @@ err_t init_uart(std::shared_ptr logg) options.c_cflag |= CREAD; // Enable receiver options.c_lflag &= ~(ICANON | ISIG); // Turn off canonical mode and signals - cfsetspeed(&options, BAUD_RATE); + cfsetspeed(&options, baud_rate); if (tcsetattr(serial_fd, TCSANOW, &options)) { @@ -50,7 +93,107 @@ err_t init_uart(std::shared_ptr logg) return OK; } -void application_task(pipe_fd_t sendpipe_fd) +static gnssrtk_gngga_t parse_gngga(std::string& NMEA_msg) +{ + gnssrtk_gngga_t gngga_msg; + std::size_t pos = 0; + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.time = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.latitude = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.latitude_hem = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.longitude = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.longitude_hem = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.status = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.number_of_sattelites = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.h_dilution_of_pos = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gngga_msg.altitude = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + return gngga_msg; +} + +static gnssrtk_gnrmc_t parse_gnrmc(std::string& NMEA_msg) +{ + gnssrtk_gnrmc_t gnrmc_msg; + std::size_t pos = 0; + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.time = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.status = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.latitude = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.latitude_hem= NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.longitude = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.longitude_hem = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.ground_speed = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.ground_heading = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.date = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.magnetic_declination = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.magnetic_declination_direction = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); + gnrmc_msg.mode = NMEA_msg.substr(0, pos); + NMEA_msg.erase(0, pos + 1); + + return gnrmc_msg; +} + +[[noreturn]] void uart_task() { char recv_buffer[GNSSRTK_DRV_RX_BUFFER_SIZE]; ioctl(serial_fd, TCFLSH, 2); // flush @@ -82,74 +225,77 @@ void application_task(pipe_fd_t sendpipe_fd) } } - auto NMEA_msg = std::string(recv_buffer); - logger->debug("GNSS message recieved: {}", NMEA_msg); + auto msg = std::string(recv_buffer); + std::size_t pos; - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto header = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto time = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto lat = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto lat_hem = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto lon = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto lon_hem = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto status = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto num_of_sattelites = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto HDOP = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - pos = NMEA_msg.find(GNSSRTK_RSP_DELIMITER); - auto alt = NMEA_msg.substr(0, pos); - NMEA_msg.erase(0, pos + 1); - - // logger->info("header: {}", header); - // logger->info("time: {}", time); - // logger->info("lat: {}", lat); - // logger->info("lat_hem: {}", lat_hem); - // logger->info("lon: {}", lon); - // logger->info("lon_hem: {}", lon_hem); - // logger->info("status: {}", status); - // logger->info("num_of_sattelites: {}", num_of_sattelites); - // logger->info("hdop: {}", HDOP); - // logger->info("alt: {}", alt); - - logger->debug("Lat: {}, Lon: {}", static_cast((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7), static_cast((std::stoi(lon.substr(0, 3)) + (std::stod(lon.substr(3)) / 60)) * 1e7)); + pos = msg.find(GNSSRTK_RSP_DELIMITER); + auto header = msg.substr(0, pos); + msg.erase(0, pos + 1); + + + + if (header == GNSSRTK_RSP_GNGGA) + { + latest_gngga.set(parse_gngga(msg)); + logger->debug("GNGGA message recieved and parsed."); + } + else if (header == GNSSRTK_RSP_GNRMC) + { + latest_gnrmc.set(parse_gnrmc(msg)); + logger->debug("GNRMC message recieved and parsed."); + } + else + { + logger->warn("Unknown NMEA-header: {}", header); + } + } +} + +[[noreturn]] void mavlink_task(pipe_fd_t send_pipe) +{ + while (true) + { + THREAD_SLEEP(GNSS_MAVLINK_PERIOD); + + auto gngga = latest_gngga.get(); + auto gnrmc = latest_gnrmc.get(); + + if (gngga.latitude != gnrmc.latitude || gngga.longitude != gnrmc.longitude) + { + logger->warn("GNGGA and GNRMC position information mismatch."); + } + + logger->debug("Constructing MAVLINK messages."); mavlink_message_t mavlink_msg; mavlink_global_position_int_t position; - position.time_boot_ms = system_uptime_ms(); - position.lat = static_cast((std::stoi(lat.substr(0, 2)) + (std::stod(lat.substr(2)) / 60)) * 1e7); - position.lon = static_cast((std::stoi(lon.substr(0, 3)) + (std::stod(lon.substr(3)) / 60)) * 1e7); - position.alt = static_cast(std::stod(alt) * 1000); - position.relative_alt = INT32_MAX; + mavlink_gps_raw_int_t raw_gps; + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len; + double u_time_ms = system_uptime_ms(); + + position.time_boot_ms = static_cast(u_time_ms); + position.lat = static_cast((std::stoi(gngga.latitude.substr(0, 2)) + (std::stod(gngga.latitude.substr(2)) / 60)) * 1e7); + position.lon = static_cast((std::stoi(gngga.longitude.substr(0, 3)) + (std::stod(gngga.longitude.substr(3)) / 60)) * 1e7); + position.alt = static_cast(std::stod(gngga.altitude) * 1000); + position.relative_alt = INT32_MAX; // TODO? position.vx = INT16_MAX; position.vy = INT16_MAX; - position.vx = INT16_MAX; - position.hdg = UINT16_MAX; + position.vz = INT16_MAX; + position.hdg = UINT16_MAX; // TODO + + raw_gps.time_usec = static_cast(u_time_ms * 1000); + raw_gps.fix_type = static_cast(std::stoi(gngga.status)); // TODO this is wrong + raw_gps.lat = position.lat; + raw_gps.lon = position.lon; + raw_gps.alt = position.alt; + raw_gps.eph = static_cast(std::stod(gngga.h_dilution_of_pos) * 100); + raw_gps.epv = UINT16_MAX; + raw_gps.vel = static_cast(std::stod(gnrmc.ground_speed) * 51.44); // knots to 100 m/s + raw_gps.cog = UINT16_MAX; + raw_gps.satellites_visible = static_cast(std::stoi(gngga.number_of_sattelites)); mavlink_msg_global_position_int_encode( MAV_SYS_ID, @@ -158,12 +304,18 @@ void application_task(pipe_fd_t sendpipe_fd) &position ); - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int len; + len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg); + write(send_pipe, buffer, len); + + mavlink_msg_gps_raw_int_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &mavlink_msg, + &raw_gps + ); len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg); - write(sendpipe_fd, buffer, len); - + write(send_pipe, buffer, len); } } diff --git a/processes/GNSS_reader/include/gnssrtk.h b/processes/GNSS_reader/include/gnssrtk.h index 429eb02..a427bb1 100644 --- a/processes/GNSS_reader/include/gnssrtk.h +++ b/processes/GNSS_reader/include/gnssrtk.h @@ -1,45 +1,19 @@ #ifndef GNSSRTK_H #define GNSSRTK_H -#include #include #include - -#include +#include #include "util.h" #include "pipe_def.h" -#define GNSSRTK_RSP_INIT '$' -#define GNSSRTK_RSP_DELIMITER "," -#define GNSSRTK_RSP_GNGGA "$GNGGA" - -#define GNSSRTK_GNGGA_ELEMENT_SIZE 15 -#define GNSSRTK_GNGGA_TIME 1 -#define GNSSRTK_GNGGA_LATITUDE 2 -#define GNSSRTK_GNGGA_LATITUDE_SIDE 3 -#define GNSSRTK_GNGGA_LONGITUDE 4 -#define GNSSRTK_GNGGA_LONGITUDE_SIDE 5 -#define GNSSRTK_GNGGA_QUALITY_INDICATOR 6 -#define GNSSRTK_GNGGA_NUMBER_OF_SATELLITES 7 -#define GNSSRTK_GNGGA_H_DILUTION_OF_POS 8 -#define GNSSRTK_GNGGA_ALTITUDE 9 -#define GNSSRTK_GNGGA_ALTITUDE_UNIT 10 -#define GNSSRTK_GNGGA_GEOIDAL_SEPARATION 11 -#define GNSSRTK_GNGGA_GEOIDAL_SEPARATION_UNIT 12 -#define GNSSRTK_GNGGA_TIME_SINCE_LAST_DGPS 13 -#define GNSSRTK_GNGGA_DGPS_REFERENCE_STATION_ID 14 - -#define GNSSRTK_DUMMY 0xFF - -#define GNSSRTK_DRV_TX_BUFFER_SIZE 100 -#define GNSSRTK_DRV_RX_BUFFER_SIZE 300 - -#define BAUD_RATE B38400 -#define SERIAL_DEVICE "/dev/serial0" +err_t init_uart(std::shared_ptr, + const std::string& = "/dev/serial0", + unsigned int = B38400); -err_t init_uart(std::shared_ptr); -void application_task(pipe_fd_t); +[[noreturn]] void uart_task(); +[[noreturn]] void mavlink_task(pipe_fd_t); void close_uart(); diff --git a/processes/GNSS_reader/main.cpp b/processes/GNSS_reader/main.cpp index b905531..e72209e 100644 --- a/processes/GNSS_reader/main.cpp +++ b/processes/GNSS_reader/main.cpp @@ -7,6 +7,7 @@ #include #include #include +#include // Libs #include @@ -17,14 +18,12 @@ #include "util.h" #include "gnssrtk.h" -#define BAUD_RATE B38400 - static std::shared_ptr logger; int main(int argc, char* argv[]) { logger = spdlog::stdout_color_mt("GNSS_reader"); - logger->set_level(GNSS_READER_LOGLEVEL); + logger->set_level(spdlog::level::debug); // TODO change to glob logger->debug("Opening sendpipe for writing."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); @@ -37,7 +36,11 @@ int main(int argc, char* argv[]) } // Mainloop - application_task(sendpipe_fd); + std::thread t1{uart_task}; + std::thread t2{mavlink_task, sendpipe_fd}; + + t1.join(); + t2.join(); close_uart(); close(sendpipe_fd); diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp index 1bfa4c7..4a0ab90 100644 --- a/processes/click_reader/main.cpp +++ b/processes/click_reader/main.cpp @@ -15,7 +15,7 @@ #include #include #include -#include + #include // Local @@ -35,7 +35,7 @@ int main(int argc, char* argv[]) { logger = spdlog::stdout_color_mt("click_reader"); - logger->set_level(CLICK_LOGLEVEL); + logger->set_level(GLOB_LOGLEVEL); logger->debug("Opening sendpipe for writing."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) // Control loop while (true) { - THREAD_SLEEP(10ms); + THREAD_SLEEP(CLICK_READ_SPI_PERIOD); res = c6dofimu17_get_gyro_data(&gyro_data); t_end = std::chrono::high_resolution_clock::now(); diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp index dd78d94..4d68c3e 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -22,7 +22,7 @@ static std::shared_ptr logger; int main(int argc, char* argv[]) { logger = spdlog::stdout_color_mt("controller"); - logger->set_level(CONTROLLER_LOGLEVEL); + logger->set_level(GLOB_LOGLEVEL); /* logger->debug("Opening sendpipe for writing."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); diff --git a/processes/include/system_def.h b/processes/include/system_def.h index b659bdd..18bccf7 100644 --- a/processes/include/system_def.h +++ b/processes/include/system_def.h @@ -2,9 +2,14 @@ #define SYSTEM_DEF_H #include +#include "util.h" const uint8_t MAV_SYS_ID = 1; const uint8_t MAV_COMP_ID = MAV_COMP_ID_AUTOPILOT1; const uint8_t MAV_CHAN_ID = MAVLINK_COMM_0; +#define CLICK_READ_SPI_PERIOD 10ms +#define GNSS_MAVLINK_PERIOD 100ms +#define MAVLINK_HEARTBEAT_PERIOD 1s + #endif \ No newline at end of file diff --git a/processes/include/util.h b/processes/include/util.h index 28fbc55..f288010 100644 --- a/processes/include/util.h +++ b/processes/include/util.h @@ -3,15 +3,28 @@ #include #include +#include -#define UDP_LOGLEVEL spdlog::level::info -#define CLICK_LOGLEVEL spdlog::level::debug -#define CONTROLLER_LOGLEVEL spdlog::level::debug -#define GNSS_READER_LOGLEVEL spdlog::level::debug +#ifdef LOG_DEBUG +#define GLOB_LOGLEVEL spdlog::level::debug +#elif LOG_INFO +#define GLOB_LOGLEVEL spdlog::level::info +#elif LOG_WARN +#define GLOB_LOGLEVEL spdlog::level::warn +#elif LOG_ERR +#define GLOB_LOGLEVEL spdlog::level::err +#else +#define GLOB_LOGLEVEL spdlog::level::info +#endif #define THREAD_SLEEP(time) std::this_thread::sleep_for(time) using namespace std::chrono_literals; +namespace spdlog +{ + class logger; // Forward declaration for logger +} + #ifndef ERR_T #define ERR_T typedef enum { @@ -20,6 +33,49 @@ typedef enum { } err_t; #endif +template +class AtomicWrap { +public: + AtomicWrap() = default; + AtomicWrap(const T& val) : data(val) {}; + ~AtomicWrap() = default; + + T get() const + { + std::lock_guard lock(mtx); + return data; + } + + void set(const T& val) + { + std::lock_guard lock(mtx); + data = val; + } + + T change(const T& val) + { + std::lock_guard lock(mtx); + T old = data; + data = val; + return old; + } + + void operator=(const T& val) { + set(val); + } + + void lock() { + mtx.lock(); + } + void unlock() { + mtx.unlock(); + } + +private: + T data; + mutable std::mutex mtx; +}; + double system_uptime_ms(); #endif \ No newline at end of file diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp index 6c67a9d..c02a5b3 100755 --- a/processes/udp_server/main.cpp +++ b/processes/udp_server/main.cpp @@ -22,7 +22,7 @@ static std::shared_ptr logger; int main(int argc, char* argv[]) { logger = spdlog::stdout_color_mt("udp_server"); - logger->set_level(UDP_LOGLEVEL); + logger->set_level(GLOB_LOGLEVEL); logger->debug("Opening pipes, waiting for writing process to start."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_RDONLY); @@ -39,6 +39,6 @@ int main(int argc, char* argv[]) logger->debug("Closing recvpipe."); close(recvpipe_fd); - return -1; + return 1; } diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 04a3d06..9f19a41 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -136,7 +136,7 @@ static void heartbeat_thread() logger->debug("Sent Heartbeat!"); } - THREAD_SLEEP(1s); + THREAD_SLEEP(MAVLINK_HEARTBEAT_PERIOD); } } -- 2.30.2