From 9c1af77755c9f19a399b1cbcabcd38ced2cf4df2 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Thu, 19 Jun 2025 00:06:13 +0200 Subject: [PATCH] restructuring and simplifying includes --- CMakeLists.txt | 1 + README.md | 4 +- processes/CMakeLists.txt | 1 - processes/GNSS_reader/gnssrtk.cpp | 6 +- processes/GNSS_reader/include/gnssrtk.h | 3 +- processes/GNSS_reader/main.cpp | 2 +- processes/click_reader/CMakeLists.txt | 16 -- processes/click_reader/main.cpp | 200 ------------------ .../{click_reader => controller}/6dofimu17.c | 3 + processes/controller/CMakeLists.txt | 6 + .../controller.cpp | 7 + .../include/6dofimu17.h | 8 +- .../include/controller.h | 11 +- processes/controller/main.cpp | 187 ++++++++++++++-- processes/include/pipe_def.h | 2 - processes/inter_proc/main.cpp | 1 - processes/udp_server/include/udp_server.h | 22 +- processes/udp_server/main.cpp | 6 +- processes/udp_server/udp_server.cpp | 23 +- run.sh | 5 +- 20 files changed, 216 insertions(+), 298 deletions(-) delete mode 100644 processes/click_reader/CMakeLists.txt delete mode 100644 processes/click_reader/main.cpp rename processes/{click_reader => controller}/6dofimu17.c (99%) mode change 100755 => 100644 processes/controller/CMakeLists.txt rename processes/{click_reader => controller}/controller.cpp (94%) rename processes/{click_reader => controller}/include/6dofimu17.h (99%) rename processes/{click_reader => controller}/include/controller.h (87%) diff --git a/CMakeLists.txt b/CMakeLists.txt index e10f23c..e8730d1 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ add_definitions(-DLOG_INFO) find_library(BCM2835_LIB bcm2835) add_subdirectory(lib/mavlink) add_subdirectory(lib/spdlog) + set(Eigen3_DIR /usr/share/eigen/cmake) find_package(Eigen3 CONFIG REQUIRED) diff --git a/README.md b/README.md index ff3f365..532b720 100755 --- a/README.md +++ b/README.md @@ -30,4 +30,6 @@ just run wvdial, correct settings should be in `/etc/wvdial.conf`. - 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? \ No newline at end of file + - Where is the controll-loop, autonomous?, how should xbox-controller inputs be handled? + - See compiler-warnings? + - controller process vs controller in click-reader? \ No newline at end of file diff --git a/processes/CMakeLists.txt b/processes/CMakeLists.txt index cfbba58..2505a60 100644 --- a/processes/CMakeLists.txt +++ b/processes/CMakeLists.txt @@ -1,5 +1,4 @@ add_subdirectory(udp_server) -add_subdirectory(click_reader) add_subdirectory(inter_proc) add_subdirectory(controller) add_subdirectory(GNSS_reader) diff --git a/processes/GNSS_reader/gnssrtk.cpp b/processes/GNSS_reader/gnssrtk.cpp index 59bea9e..b15238c 100644 --- a/processes/GNSS_reader/gnssrtk.cpp +++ b/processes/GNSS_reader/gnssrtk.cpp @@ -2,15 +2,17 @@ #include #include -#include #include +#include #include +#include #include #include #include "system_def.h" +#include "util.h" static const char GNSSRTK_RSP_INIT ='$'; static const char GNSSRTK_RSP_DELIMITER = ','; @@ -252,7 +254,7 @@ static gnssrtk_gnrmc_t parse_gnrmc(std::string& NMEA_msg) } } -[[noreturn]] void mavlink_task(pipe_fd_t send_pipe) +[[noreturn]] void mavlink_task(int send_pipe) { while (true) { diff --git a/processes/GNSS_reader/include/gnssrtk.h b/processes/GNSS_reader/include/gnssrtk.h index a427bb1..57d80bb 100644 --- a/processes/GNSS_reader/include/gnssrtk.h +++ b/processes/GNSS_reader/include/gnssrtk.h @@ -6,14 +6,13 @@ #include #include "util.h" -#include "pipe_def.h" err_t init_uart(std::shared_ptr, const std::string& = "/dev/serial0", unsigned int = B38400); [[noreturn]] void uart_task(); -[[noreturn]] void mavlink_task(pipe_fd_t); +[[noreturn]] void mavlink_task(int); void close_uart(); diff --git a/processes/GNSS_reader/main.cpp b/processes/GNSS_reader/main.cpp index e72209e..57d78c8 100644 --- a/processes/GNSS_reader/main.cpp +++ b/processes/GNSS_reader/main.cpp @@ -26,7 +26,7 @@ int main(int argc, char* argv[]) 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); + int sendpipe_fd = open(SEND_PIPE, O_WRONLY); logger->info("Initializing UART."); int ret = init_uart(logger); diff --git a/processes/click_reader/CMakeLists.txt b/processes/click_reader/CMakeLists.txt deleted file mode 100644 index 582866d..0000000 --- a/processes/click_reader/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -add_executable(click_reader - main.cpp - 6dofimu17.c - controller.cpp -) - -target_include_directories(click_reader PRIVATE include) - -target_link_libraries(click_reader PRIVATE MAVLink) -target_link_libraries(click_reader PRIVATE spdlog::spdlog) -target_link_libraries(click_reader PRIVATE Eigen3::Eigen) -target_link_libraries(click_reader PRIVATE common_lib) - -target_link_libraries(click_reader PRIVATE ${BCM2835_LIB}) - -target_include_directories(click_reader PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp deleted file mode 100644 index 4a0ab90..0000000 --- a/processes/click_reader/main.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// Standard -#include -#include -#include -#include -#include - -// Pipes -#include -#include -#include -#include - -// Libs -#include -#include -#include - -#include - -// Local -#include "pipe_def.h" -#include "util.h" -#include "6dofimu17.h" -#include "controller.h" -#include "system_def.h" - -static std::shared_ptr logger; - -static err_t spi_init(); -static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len); -static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len); - -int main(int argc, char* argv[]) -{ - - logger = spdlog::stdout_color_mt("click_reader"); - logger->set_level(GLOB_LOGLEVEL); - - logger->debug("Opening sendpipe for writing."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); - - logger->info("Initializing BCM2835 and SPI."); - if (spi_init() != OK) - { - logger->error("spi_init() failed. Are you running as root?"); - return 1; - } - - logger->debug("Initializing 6DOF IMU 17 Click."); - c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); - - c6dofimu17_gyro_cfg_t gyro_cfg; - c6dofimu17_accel_cfg_t accel_cfg; - - accel_cfg.accel_fs_sel = C6DOFIMU17_SET_ACCEL_FS_SEL_16g; // Drones typically between 0-15g? - accel_cfg.accel_odr = C6DOFIMU17_SET_ACCEL_ODR_1kHz; - accel_cfg.accel_dec2_m2_ord = C6DOFIMU17_SET_ACCEL_DEC2_M2_ORD_3st; - accel_cfg.accel_ui_filt_ord = C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_3st; - accel_cfg.accel_ui_filt_bw = C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_20; // ~50Hz - - gyro_cfg.gyro_fs_sel = C6DOFIMU17_SET_GYRO_FS_SEL_1000_dps; // Drones typically between 0-500 deg/s? - gyro_cfg.gyro_odr = C6DOFIMU17_SET_GYRO_ODR_1kHz; - gyro_cfg.gyro_ui_filt_ord = C6DOFIMU17_SET_GYRO_UI_FILT_ORD_3st; - gyro_cfg.gyro_dec2_m2_ord = C6DOFIMU17_SET_GYRO_DEC2_M2_ORD_3st; - gyro_cfg.gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz - - c6dofimu17_cfg(&gyro_cfg, &accel_cfg); - - c6dofimu17_axis_t accel_data; - c6dofimu17_axis_t gyro_data; - double temperature = 0.0; - - Complementary_filter comp_filter{logger, 0.02}; - - int res = 0; - auto t_start = std::chrono::high_resolution_clock::now(); - auto t_end = std::chrono::high_resolution_clock::now(); - double time_d = 0.0; - - mavlink_message_t msg; - mavlink_attitude_t attitude; - mavlink_scaled_imu_t scaled_imu; - // Control loop - while (true) - { - THREAD_SLEEP(CLICK_READ_SPI_PERIOD); - - res = c6dofimu17_get_gyro_data(&gyro_data); - t_end = std::chrono::high_resolution_clock::now(); - time_d = std::chrono::duration(t_end-t_start).count(); - t_start = std::chrono::high_resolution_clock::now(); - - res += c6dofimu17_get_accel_data(&accel_data); - - res += c6dofimu17_get_temperature(&temperature); - - if (res != OK) { - logger->warn("Failed to get imu data: {}", res); - continue; - } - - vec3d gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); - vec3d accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); - - comp_filter.update(gyro_vec, accel_vec, time_d); - - // Pack data into MAVLINK packages - attitude.time_boot_ms = system_uptime_ms(); - attitude.roll = comp_filter.get_state()[1]; - attitude.pitch = comp_filter.get_state()[0]; - attitude.yaw = 0.0; - attitude.rollspeed = 0.0; - attitude.pitchspeed = 0.0; - attitude.yawspeed = 0.0; - - scaled_imu.time_boot_ms = system_uptime_ms(); - scaled_imu.temperature = static_cast(temperature * 100); - scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? - scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? - scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? - scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? - scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? - scaled_imu.zgyro = static_cast(gyro_vec[2] * 1000); // Maybe not correct? - scaled_imu.xmag = INT16_MAX; - scaled_imu.ymag = INT16_MAX; - scaled_imu.zmag = INT16_MAX; - - mavlink_msg_attitude_encode( - MAV_SYS_ID, - MAV_COMP_ID, - &msg, - &attitude - ); - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int len; - - len = mavlink_msg_to_send_buffer(buffer, &msg); - write(sendpipe_fd, buffer, len); - - mavlink_msg_scaled_imu_encode( - MAV_SYS_ID, - MAV_COMP_ID, - &msg, - &scaled_imu - ); - len = mavlink_msg_to_send_buffer(buffer, &msg); - write(sendpipe_fd, buffer, len); - } - - close(sendpipe_fd); - return 0; -} - -// Some good old C! -static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len) -{ - char buffer[len + 1]; - memset(buffer, 0, len + 1); - buffer[0] = reg; - memcpy(buffer + 1, data_out, len); - - bcm2835_spi_writenb(buffer, len + 1); - return OK; -} - -// Some good old C! -static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len) -{ - char buffer[len + 1]; - memset(buffer, 0, len + 1); - buffer[0] = reg | 0x80; - - bcm2835_spi_transfern(buffer, len + 1); - - memcpy(data_in, buffer + 1, len); - return OK; -} - -static err_t spi_init() -{ - if (!bcm2835_init()) - { - return ERR; - } - - if (!bcm2835_spi_begin()) - { - return ERR; - } - - bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST); // The default - bcm2835_spi_setDataMode(BCM2835_SPI_MODE0); // The default - bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_32); // The default - bcm2835_spi_chipSelect(BCM2835_SPI_CS0); // The default - bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default - - return OK; -} \ No newline at end of file diff --git a/processes/click_reader/6dofimu17.c b/processes/controller/6dofimu17.c similarity index 99% rename from processes/click_reader/6dofimu17.c rename to processes/controller/6dofimu17.c index 37ffaf0..204e639 100644 --- a/processes/click_reader/6dofimu17.c +++ b/processes/controller/6dofimu17.c @@ -28,6 +28,9 @@ #include "6dofimu17.h" #include +#include + +#include "c_util.h" /** * @brief Dummy data. diff --git a/processes/controller/CMakeLists.txt b/processes/controller/CMakeLists.txt old mode 100755 new mode 100644 index 1bd5bd9..d59d546 --- a/processes/controller/CMakeLists.txt +++ b/processes/controller/CMakeLists.txt @@ -1,10 +1,16 @@ add_executable(controller main.cpp + 6dofimu17.c + controller.cpp ) target_include_directories(controller PRIVATE include) target_link_libraries(controller PRIVATE MAVLink) target_link_libraries(controller PRIVATE spdlog::spdlog) +target_link_libraries(controller PRIVATE Eigen3::Eigen) +target_link_libraries(controller PRIVATE common_lib) + +target_link_libraries(controller PRIVATE ${BCM2835_LIB}) target_include_directories(controller PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include) diff --git a/processes/click_reader/controller.cpp b/processes/controller/controller.cpp similarity index 94% rename from processes/click_reader/controller.cpp rename to processes/controller/controller.cpp index eacfcb0..79ff0f2 100644 --- a/processes/click_reader/controller.cpp +++ b/processes/controller/controller.cpp @@ -1,6 +1,13 @@ #include "controller.h" #include +#include + +#include +#include + +#include "6dofimu17.h" +#include "util.h" static vec2d pqr_to_euler(const vec3d&, const vec2d&); static vec2d accel_to_pitch_roll(const vec3d&); diff --git a/processes/click_reader/include/6dofimu17.h b/processes/controller/include/6dofimu17.h similarity index 99% rename from processes/click_reader/include/6dofimu17.h rename to processes/controller/include/6dofimu17.h index 6ed01da..95e1c29 100644 --- a/processes/click_reader/include/6dofimu17.h +++ b/processes/controller/include/6dofimu17.h @@ -32,6 +32,10 @@ extern "C"{ #endif +#include + +#include "c_util.h" + // Register definitions #define C6DOFIMU17_REG_DEVICE_CONFIG 0x11 #define C6DOFIMU17_REG_DRIVE_CONFIG 0x13 @@ -242,10 +246,6 @@ extern "C"{ #define C6DOFIMU17_SET_BANK_3 0x03 #define C6DOFIMU17_SET_BANK_4 0x04 -#include - -#include - typedef err_t ( *c6dofimu17_master_io_t )( uint8_t, uint8_t*, uint8_t ); /**< Driver serial interface. */ typedef struct { diff --git a/processes/click_reader/include/controller.h b/processes/controller/include/controller.h similarity index 87% rename from processes/click_reader/include/controller.h rename to processes/controller/include/controller.h index 389c689..8cfc66b 100644 --- a/processes/click_reader/include/controller.h +++ b/processes/controller/include/controller.h @@ -1,19 +1,14 @@ #ifndef CONTROLLER_H #define CONTROLLER_H -// Standard #include -#include - -// Libs + #include -#include -#include -// Local +#include "util.h" #include "6dofimu17.h" -#define GRAVITY 9.82f +const double GRAVITY = 9.82f; typedef Eigen::Vector3d vec3d; typedef Eigen::Vector2d vec2d; diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp index 4d68c3e..fe89c61 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -1,6 +1,8 @@ // Standard #include -#include +#include +#include +#include // Pipes #include @@ -12,42 +14,185 @@ #include #include #include +#include // Local #include "pipe_def.h" #include "util.h" +#include "6dofimu17.h" +#include "controller.h" +#include "system_def.h" static std::shared_ptr logger; +static err_t spi_init(); +static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len); +static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len); + int main(int argc, char* argv[]) { - logger = spdlog::stdout_color_mt("controller"); + + logger = spdlog::stdout_color_mt("click_reader"); logger->set_level(GLOB_LOGLEVEL); -/* + logger->debug("Opening sendpipe for writing."); - pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); -*/ - logger->debug("Opening recvpipe for reading."); - pipe_fd_t recvpipe_fd = open(RECV_PIPE, O_RDONLY); + int sendpipe_fd = open(SEND_PIPE, O_WRONLY); - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - while(true) + logger->info("Initializing BCM2835 and SPI."); + if (spi_init() != OK) { - int ret = read(recvpipe_fd, buffer, MAVLINK_MAX_PACKET_LEN); + logger->error("spi_init() failed. Are you running as root?"); + return 1; + } - if (ret == 0) { - logger->error("No writing process on recvpipe. Exiting."); - break; - } - else if (ret < 0) - logger->error("Error while reading message. {}", strerror(errno)); - else { - // handle mavlink message + logger->debug("Initializing 6DOF IMU 17 Click."); + c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); + + c6dofimu17_gyro_cfg_t gyro_cfg; + c6dofimu17_accel_cfg_t accel_cfg; + + accel_cfg.accel_fs_sel = C6DOFIMU17_SET_ACCEL_FS_SEL_16g; // Drones typically between 0-15g? + accel_cfg.accel_odr = C6DOFIMU17_SET_ACCEL_ODR_1kHz; + accel_cfg.accel_dec2_m2_ord = C6DOFIMU17_SET_ACCEL_DEC2_M2_ORD_3st; + accel_cfg.accel_ui_filt_ord = C6DOFIMU17_SET_ACCEL_UI_FILT_ORD_3st; + accel_cfg.accel_ui_filt_bw = C6DOFIMU17_SET_ACCEL_UI_FILT_BW_ODR_20; // ~50Hz + + gyro_cfg.gyro_fs_sel = C6DOFIMU17_SET_GYRO_FS_SEL_1000_dps; // Drones typically between 0-500 deg/s? + gyro_cfg.gyro_odr = C6DOFIMU17_SET_GYRO_ODR_1kHz; + gyro_cfg.gyro_ui_filt_ord = C6DOFIMU17_SET_GYRO_UI_FILT_ORD_3st; + gyro_cfg.gyro_dec2_m2_ord = C6DOFIMU17_SET_GYRO_DEC2_M2_ORD_3st; + gyro_cfg.gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz + + c6dofimu17_cfg(&gyro_cfg, &accel_cfg); + + c6dofimu17_axis_t accel_data; + c6dofimu17_axis_t gyro_data; + double temperature = 0.0; + + Complementary_filter comp_filter{logger, 0.02}; + + int res = 0; + auto t_start = std::chrono::high_resolution_clock::now(); + auto t_end = std::chrono::high_resolution_clock::now(); + double time_d = 0.0; + + mavlink_message_t msg; + mavlink_attitude_t attitude; + mavlink_scaled_imu_t scaled_imu; + // Control loop + while (true) + { + THREAD_SLEEP(CLICK_READ_SPI_PERIOD); + + res = c6dofimu17_get_gyro_data(&gyro_data); + t_end = std::chrono::high_resolution_clock::now(); + time_d = std::chrono::duration(t_end-t_start).count(); + t_start = std::chrono::high_resolution_clock::now(); + + res += c6dofimu17_get_accel_data(&accel_data); + + res += c6dofimu17_get_temperature(&temperature); + + if (res != OK) { + logger->warn("Failed to get imu data: {}", res); + continue; } + + vec3d gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); + vec3d accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); + + comp_filter.update(gyro_vec, accel_vec, time_d); + + // Pack data into MAVLINK packages + attitude.time_boot_ms = system_uptime_ms(); + attitude.roll = comp_filter.get_state()[1]; + attitude.pitch = comp_filter.get_state()[0]; + attitude.yaw = 0.0; + attitude.rollspeed = 0.0; + attitude.pitchspeed = 0.0; + attitude.yawspeed = 0.0; + + scaled_imu.time_boot_ms = system_uptime_ms(); + scaled_imu.temperature = static_cast(temperature * 100); + scaled_imu.xacc = static_cast(accel_vec[0] * 1000); // Maybe not correct? + scaled_imu.yacc = static_cast(accel_vec[1] * 1000); // Maybe not correct? + scaled_imu.zacc = static_cast(accel_vec[2] * 1000); // Maybe not correct? + scaled_imu.xgyro = static_cast(gyro_vec[0] * 1000); // Maybe not correct? + scaled_imu.ygyro = static_cast(gyro_vec[1] * 1000); // Maybe not correct? + scaled_imu.zgyro = static_cast(gyro_vec[2] * 1000); // Maybe not correct? + scaled_imu.xmag = INT16_MAX; + scaled_imu.ymag = INT16_MAX; + scaled_imu.zmag = INT16_MAX; + + mavlink_msg_attitude_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &msg, + &attitude + ); + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int len; + + len = mavlink_msg_to_send_buffer(buffer, &msg); + write(sendpipe_fd, buffer, len); + + mavlink_msg_scaled_imu_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &msg, + &scaled_imu + ); + len = mavlink_msg_to_send_buffer(buffer, &msg); + write(sendpipe_fd, buffer, len); } - logger->error("here"); - //close(sendpipe_fd); - close(recvpipe_fd); + close(sendpipe_fd); return 0; +} + +// Some good old C! +static err_t spi_write_alias(uint8_t reg, uint8_t* data_out, uint8_t len) +{ + char buffer[len + 1]; + memset(buffer, 0, len + 1); + buffer[0] = reg; + memcpy(buffer + 1, data_out, len); + + bcm2835_spi_writenb(buffer, len + 1); + return OK; +} + +// Some good old C! +static err_t spi_read_alias(uint8_t reg, uint8_t* data_in, uint8_t len) +{ + char buffer[len + 1]; + memset(buffer, 0, len + 1); + buffer[0] = reg | 0x80; + + bcm2835_spi_transfern(buffer, len + 1); + + memcpy(data_in, buffer + 1, len); + return OK; +} + +static err_t spi_init() +{ + if (!bcm2835_init()) + { + return ERR; + } + + if (!bcm2835_spi_begin()) + { + return ERR; + } + + bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST); // The default + bcm2835_spi_setDataMode(BCM2835_SPI_MODE0); // The default + bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_32); // The default + bcm2835_spi_chipSelect(BCM2835_SPI_CS0); // The default + bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); // the default + + return OK; } \ No newline at end of file diff --git a/processes/include/pipe_def.h b/processes/include/pipe_def.h index f34774e..4c518fc 100644 --- a/processes/include/pipe_def.h +++ b/processes/include/pipe_def.h @@ -4,6 +4,4 @@ #define RECV_PIPE "/var/lock/recv_pipe" #define SEND_PIPE "/var/lock/send_pipe" -typedef int pipe_fd_t; - #endif \ No newline at end of file diff --git a/processes/inter_proc/main.cpp b/processes/inter_proc/main.cpp index 0b0ec33..dc361e9 100644 --- a/processes/inter_proc/main.cpp +++ b/processes/inter_proc/main.cpp @@ -7,7 +7,6 @@ #include #include - #include "pipe_def.h" static std::shared_ptr logger; diff --git a/processes/udp_server/include/udp_server.h b/processes/udp_server/include/udp_server.h index 6902b61..6e098ac 100644 --- a/processes/udp_server/include/udp_server.h +++ b/processes/udp_server/include/udp_server.h @@ -1,31 +1,11 @@ #ifndef UDP_SERVER_H #define UDP_SERVER_H -// Standard #include -// Socket -#include -#include - -// Libs -#include - -// Local -#include "pipe_def.h" #include "util.h" -#define UDP_PORT 14552 -#define UDP_RECV_BUF_SIZE 2048 // enough for MTU 1500 bytes - -typedef struct { - int socket_fd; - struct sockaddr_in addr; - socklen_t addr_len; - bool addr_set; -} gcs_t; - -err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd); +err_t udp_server_init(std::shared_ptr logg, const int recvpipe_fd, const int sendpipe_fd); err_t udp_server_run(); #endif \ No newline at end of file diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp index c02a5b3..af94c11 100755 --- a/processes/udp_server/main.cpp +++ b/processes/udp_server/main.cpp @@ -1,6 +1,5 @@ // Standard #include -#include // Pipes #include @@ -9,7 +8,6 @@ // Libs #include #include -#include // Local #include "pipe_def.h" @@ -25,8 +23,8 @@ int main(int argc, char* argv[]) 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); - pipe_fd_t recvpipe_fd = open(RECV_PIPE, O_WRONLY); + int sendpipe_fd = open(SEND_PIPE, O_RDONLY); + int recvpipe_fd = open(RECV_PIPE, O_WRONLY); udp_server_init(logger, recvpipe_fd, sendpipe_fd); diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 9f19a41..f534dea 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -1,33 +1,36 @@ #include "udp_server.h" -// Standard #include #include -// Socket #include #include #include - -// Pipes #include -// Libs #include #include -// Local #include "system_def.h" -#include "pipe_def.h" #include "util.h" static std::shared_ptr logger; +static const int UDP_PORT = 14552; +static const int UDP_RECV_BUF_SIZE = 2048; // enough for MTU 1500 bytes + +typedef struct { + int socket_fd; + struct sockaddr_in addr; + socklen_t addr_len; + bool addr_set; +} gcs_t; + static bool recv_thread_running = true; static gcs_t gcs = {}; -static pipe_fd_t recvpipe; -static pipe_fd_t sendpipe; +static int recvpipe; +static int sendpipe; // Declarations static void handle_request_autopilot_capabilities(const mavlink_command_long_t& cmd_long); @@ -172,7 +175,7 @@ static void handle_mavlink(mavlink_message_t& message) } } -err_t udp_server_init(std::shared_ptr logg, const pipe_fd_t recvpipe_fd, const pipe_fd_t sendpipe_fd) +err_t udp_server_init(std::shared_ptr logg, const int recvpipe_fd, const int sendpipe_fd) { logger = logg; sendpipe = sendpipe_fd; diff --git a/run.sh b/run.sh index deadd0d..07a430f 100755 --- a/run.sh +++ b/run.sh @@ -26,11 +26,8 @@ sleep 1 # start udp server ./udp_server & -# start controller -./controller & - # start click reader -./click_reader & +./controller & # start GNSS reader ./GNSS_reader -- 2.30.2