From 325a999f4ac1455fd4d3015274603234a86249b2 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Sun, 5 Jul 2026 03:21:15 +0200 Subject: [PATCH] Joystick forwarding working --- CMakeLists.txt | 4 +- modem.sh | 7 + processes/controller/6dofimu17.c | 13 ++ processes/controller/controller.cpp | 19 +- processes/controller/include/6dofimu17.h | 2 + processes/controller/include/controller.h | 42 ++-- processes/controller/main.cpp | 221 +++++++++++++++++----- processes/include/system_def.h | 15 ++ processes/udp_server/main.cpp | 2 +- processes/udp_server/udp_server.cpp | 27 ++- 10 files changed, 269 insertions(+), 83 deletions(-) create mode 100755 modem.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index e8730d1..87f6205 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,13 +7,15 @@ SET(GCC_COVERAGE_COMPILE_FLAGS "-Wall -march=armv8-a+crc -mcpu=cortex-a72 -mtune SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS}") SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${GCC_COVERAGE_LINK_FLAGS}") +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) 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) +# add_definitions(-DLOG_ERR) find_library(BCM2835_LIB bcm2835) add_subdirectory(lib/mavlink) diff --git a/modem.sh b/modem.sh new file mode 100755 index 0000000..6cbe686 --- /dev/null +++ b/modem.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +# nmcli should be set up so that wwan0 shows up default (named telia) + +sudo ip addr add 10.37.129.125/30 dev wwan0 +sudo ip link set wwan0 up +sudo ip route add default via 10.37.129.126 dev wwan0 diff --git a/processes/controller/6dofimu17.c b/processes/controller/6dofimu17.c index 204e639..9facc59 100644 --- a/processes/controller/6dofimu17.c +++ b/processes/controller/6dofimu17.c @@ -251,6 +251,19 @@ err_t c6dofimu17_soft_reset ( void ) return err_flag; } +err_t c6dofimu17_signal_path_reset( void ) +{ + uint8_t tmp; + + err_t err_flag = c6dofimu17_generic_read( C6DOFIMU17_REG_SIGNAL_PATH_RESET, &tmp, 1 ); + + tmp |= 0x08; + + err_flag |= c6dofimu17_generic_write( C6DOFIMU17_REG_SIGNAL_PATH_RESET, &tmp, 1 ); + + return err_flag; +} + err_t c6dofimu17_get_accel_data ( c6dofimu17_axis_t *accel_data ) { uint8_t rx_buf[ 6 ]; diff --git a/processes/controller/controller.cpp b/processes/controller/controller.cpp index 827b0de..a02524f 100644 --- a/processes/controller/controller.cpp +++ b/processes/controller/controller.cpp @@ -21,20 +21,13 @@ vec3d struct_to_vec_scaled(const c6dofimu17_axis_t& data, const double scale) return vec3d{data_x, data_y, data_z}; } - Complementary_filter::Complementary_filter(std::shared_ptr logg, const double alpha) : - logger{logg}, - alpha{alpha}, - current_state{vec2d::Zero()} + Abstract_Filter{logg}, + alpha{alpha} {} -const vec2d Complementary_filter::get_state() -{ - return current_state; -} - void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_meas, - double dt_sec, unsigned int gyro_range, unsigned int accel_range) + double dt_sec) { auto accel_est = accel_to_pitch_roll(accel_meas); auto gyro_est = pqr_to_euler(gyro_meas, current_state); @@ -42,6 +35,11 @@ void Complementary_filter::update(const vec3d &gyro_meas, const vec3d &accel_mea current_state = (alpha * accel_est) + (1 - alpha) * (current_state + (dt_sec * gyro_est)); } +void Complementary_filter::reset() +{ + current_state = vec2d::Zero(); +} + static vec2d pqr_to_euler(const vec3d& pqr, const vec2d& pitch_roll) { double phi = pitch_roll[0]; @@ -61,5 +59,4 @@ static vec2d accel_to_pitch_roll(const vec3d& accel) atan2(accel[1], accel[2]), atan2(-accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))) }; - } \ No newline at end of file diff --git a/processes/controller/include/6dofimu17.h b/processes/controller/include/6dofimu17.h index 95e1c29..f72a850 100644 --- a/processes/controller/include/6dofimu17.h +++ b/processes/controller/include/6dofimu17.h @@ -309,6 +309,8 @@ err_t c6dofimu17_get_config_accel ( c6dofimu17_accel_cfg_t *accel_cfg ); err_t c6dofimu17_soft_reset ( void ); +err_t c6dofimu17_signal_path_reset ( void ); + err_t c6dofimu17_get_accel_data ( c6dofimu17_axis_t *accel_data ); err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data ); diff --git a/processes/controller/include/controller.h b/processes/controller/include/controller.h index 2ec06ea..7a2d04e 100644 --- a/processes/controller/include/controller.h +++ b/processes/controller/include/controller.h @@ -15,35 +15,51 @@ typedef Eigen::Vector2d vec2d; vec3d struct_to_vec_scaled(const c6dofimu17_axis_t&, const double); -class Complementary_filter +class Abstract_Filter +{ +public: + Abstract_Filter(std::shared_ptr logg) : + logger{logg}, + current_state{vec2d::Zero()} + {} + ~Abstract_Filter() = default; + + virtual void update(const vec3d&, const vec3d&, double) = 0; + + const vec2d get_state() { + return current_state; + } + + virtual void reset() = 0; + +protected: + std::shared_ptr logger; + vec2d current_state; // Pitch and roll +}; + +class Complementary_filter : public Abstract_Filter { public: Complementary_filter(std::shared_ptr, const double = 0.02); - void update(const vec3d&, const vec3d&, - double, const unsigned int = 1000, const unsigned int = 16); + void update(const vec3d&, const vec3d&, double) override; - const vec2d get_state(); + void reset() override; private: - std::shared_ptr logger; const double alpha; - vec2d current_state; }; -class EKF +class EKF : public Abstract_Filter { public: EKF(std::shared_ptr); - void update(const vec3d&, const vec3d&, - double); - - const vec2d get_state(); + void update(const vec3d&, const vec3d&, double) override; + void reset() override; private: - std::shared_ptr logger; - vec2d current_state; }; + #endif \ No newline at end of file diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp index 877b5b2..45e0551 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -28,17 +28,29 @@ static std::shared_ptr logger; +static err_t c6dofimu17_cfg(); 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); +static err_t spi_write_alias(uint8_t, uint8_t*, uint8_t); +static err_t spi_read_alias(uint8_t, uint8_t*, uint8_t); [[noreturn]] static void send_mavlink(int); +[[noreturn]] static void imu_controller(); +[[noreturn]] static void mavlink_handler(int); +[[noreturn]] static void controller(); static vec3d gyro_vec; static vec3d accel_vec; static double temperature = 0.0; static Complementary_filter comp_filter{logger, 0.02}; -static std::mutex data_lock; +static std::mutex imu_data_lock; + +static uint16_t joystick_left_x = 0.0; +static uint16_t joystick_left_y = 0.0; +static uint16_t joystick_right_x = 0.0; +static uint16_t joystick_right_y = 0.0; +static uint16_t trigger = 0.0; + +static std::mutex joystick_data_lock; int main(int argc, char* argv[]) { @@ -59,6 +71,30 @@ int main(int argc, char* argv[]) } logger->debug("Initializing 6DOF IMU 17 Click."); + if (c6dofimu17_cfg() != OK) + { + logger->error("IMU cfg init failed."); + return 1; + } + + // Starting threads + std::thread mavlink_task{send_mavlink, sendpipe_fd}; + std::thread imu_task{imu_controller}; + std::thread mavlink_handler_task{mavlink_handler, recvpipe_fd}; + std::thread controller_task{controller}; + + + mavlink_task.join(); + imu_task.join(); + mavlink_handler_task.join(); + controller_task.join(); + + close(sendpipe_fd); + return 0; +} + +static err_t c6dofimu17_cfg() +{ c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); c6dofimu17_gyro_cfg_t gyro_cfg; @@ -77,51 +113,7 @@ int main(int argc, char* argv[]) gyro_cfg.gyro_ui_filt_bw = C6DOFIMU17_SET_GYRO_UI_FILT_BW_ODR_20; // ~50Hz c6dofimu17_cfg(&gyro_cfg, &accel_cfg); - - 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; - - c6dofimu17_axis_t accel_data; - c6dofimu17_axis_t gyro_data; - - // Mavlink thread - std::thread mavlink_task{send_mavlink, sendpipe_fd}; - - // Control loop - while (true) - { - THREAD_SLEEP(CLICK_READ_SPI_PERIOD); - - /* __________________________________________________________ */ - std::lock_guard lk{data_lock}; - - logger->debug("Controlling!"); - - 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; - } - - gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); - accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); - - comp_filter.update(gyro_vec, accel_vec, time_d); - - /* __________________________________________________________ */ - } - - close(sendpipe_fd); - return 0; + return OK; } // Some good old C! @@ -182,7 +174,7 @@ static err_t spi_init() logger->debug("Sending IMU Mavlink."); /* __________________________________________________________ */ - data_lock.lock(); + imu_data_lock.lock(); // Pack data into MAVLINK packages attitude.time_boot_ms = system_uptime_ms(); @@ -205,7 +197,7 @@ static err_t spi_init() scaled_imu.ymag = INT16_MAX; scaled_imu.zmag = INT16_MAX; - data_lock.unlock(); + imu_data_lock.unlock(); /* __________________________________________________________ */ mavlink_msg_attitude_encode( @@ -230,4 +222,131 @@ static err_t spi_init() len = mavlink_msg_to_send_buffer(buffer, &msg); write(sendpipe_fd, buffer, len); } -} \ No newline at end of file +} + +[[noreturn]] static void imu_controller() +{ + 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; + + c6dofimu17_axis_t accel_data; + c6dofimu17_axis_t gyro_data; + + while (true) + { + THREAD_SLEEP(CLICK_READ_SPI_PERIOD); + + /* __________________________________________________________ */ + std::lock_guard lk{imu_data_lock}; + + logger->debug("Reading IMU!"); + + 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; + } + + gyro_vec = struct_to_vec_scaled(gyro_data, (M_PI * 1000.0) / (32768.0 * 180.0)); + accel_vec = struct_to_vec_scaled(accel_data, (GRAVITY * 16.0) / 32768.0); + + comp_filter.update(gyro_vec, accel_vec, time_d); + + /* __________________________________________________________ */ + } +} + +[[noreturn]] static void mavlink_handler(int recvpipe_fd) +{ + int res = 0; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + mavlink_message_t message; + mavlink_status_t status; + + while (true) + { + res = read(recvpipe_fd, buffer, sizeof(buffer)); + if (res <= 0) { + logger->error("Failed to read from recvpipe: {}", strerror(errno)); + continue; + } + + for (int i = 0; i < res; i++) { + if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status)) { + + switch (message.msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&message, &cmd); + + switch (cmd.command) + { + case MAV_CMD_PREFLIGHT_CALIBRATION: + { + logger->debug("Resetting and calibrating IMU etc."); + + // Handle preflight calibration command here + std::lock_guard lk{imu_data_lock}; + + // Reset the filter state + c6dofimu17_signal_path_reset(); + comp_filter.reset(); + gyro_vec = vec3d::Zero(); + accel_vec = vec3d::Zero(); + temperature = 0.0; + break; + } + default: + logger->warn("Unhandled COMMAND_LONG: {}", cmd.command); + break; + } + break; + } + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + { + logger->debug("Handled mavlink message RC channels."); + + std::lock_guard lk{joystick_data_lock}; + + mavlink_rc_channels_override_t rc; + mavlink_msg_rc_channels_override_decode(&message, &rc); + + joystick_left_x = rc.chan1_raw; + joystick_left_y = rc.chan2_raw; + joystick_right_x = rc.chan3_raw; + joystick_right_y = rc.chan4_raw; + trigger = rc.chan5_raw; + break; + } + default: + logger->warn("Unhandled MAVLink message id: {}", (int) message.msgid); + break; + } + break; // Exit the for loop after processing a complete message + } + } + } +} + +[[noreturn]] static void controller() +{ + while (true) + { + THREAD_SLEEP(MAIN_CONTROLLER_PERIOD); + logger->info("Controlling!"); + + std::lock_guard lk{joystick_data_lock}; + logger->info("Joystick values: {}, {}, {}, {}, {}", joystick_left_x, joystick_left_y, joystick_right_x, joystick_right_y, trigger); + + } +} diff --git a/processes/include/system_def.h b/processes/include/system_def.h index d1e6695..faba23c 100644 --- a/processes/include/system_def.h +++ b/processes/include/system_def.h @@ -1,6 +1,8 @@ #ifndef SYSTEM_DEF_H #define SYSTEM_DEF_H +#include + #include #include "util.h" @@ -8,8 +10,21 @@ 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; +const std::set supported_mavlink_control_cmds = +{ + MAV_CMD_PREFLIGHT_CALIBRATION + +}; + +const std::set supported_mavlink_control_msgs = +{ + MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE + +}; + #define CLICK_READ_SPI_PERIOD 10ms #define GNSS_MAVLINK_PERIOD 100ms +#define MAIN_CONTROLLER_PERIOD 100ms #define CONTROLLER_MAVLINK_PERIOD 100ms #define MAVLINK_HEARTBEAT_PERIOD 1s diff --git a/processes/udp_server/main.cpp b/processes/udp_server/main.cpp index 75b2643..86bfc78 100755 --- a/processes/udp_server/main.cpp +++ b/processes/udp_server/main.cpp @@ -38,7 +38,7 @@ int main(int argc, char* argv[]) logger->debug("Closing recvpipe."); close(recvpipe_fd); - + return 1; } diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 5394653..8dbd7ff 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -164,13 +164,28 @@ static void handle_mavlink(mavlink_message_t& message) } break; default: - logger->warn("Unhandled message {} from {}/{}.", (int) message.msgid, message.sysid, message.compid); - - logger->warn("Sending unhandled message to recvpipe: {} bytes.", message.len); - auto ret = write(recvpipe, message.payload64, message.len); - if (ret < 0) { - logger->error("Error while writing message to recvpipe. Errno {}.", strerror(errno)); + if (supported_mavlink_control_msgs.contains(message.msgid)) + { + logger->debug("Got supported message {} from {}/{}.", (int) message.msgid, message.sysid, message.compid); + // Forward the message to the recvpipe + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + + uint16_t len = mavlink_msg_to_send_buffer(buffer, &message); + + ssize_t ret = write(recvpipe, buffer, len); + if (ret < 0) { + logger->error("Error while writing message to recvpipe. Errno {}.", strerror(errno)); + } + } + else if (supported_mavlink_control_cmds.contains(message.msgid)) + { + logger->warn("Got control (what even is a control command?) command {} from {}/{}.", (int) message.msgid, message.sysid, message.compid); } + else + { + logger->warn("Unhandled unknown {} from {}/{}.", (int) message.msgid, message.sysid, message.compid); + } + break; } } -- 2.47.3