From 73250b7c44941a0b81bbd82f63cadde3a931e6b5 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Sun, 8 Jun 2025 00:48:43 +0200 Subject: [PATCH] Complementary filter for IMU working --- CMakeLists.txt | 3 +- README.md | 29 ++-- build.sh | 2 +- cmake_build.sh | 5 +- processes/click_reader/6dofimu17.c | 4 +- processes/click_reader/CMakeLists.txt | 2 + processes/click_reader/controller.cpp | 100 ++++++++++++++ processes/click_reader/include/6dofimu17.h | 2 +- processes/click_reader/include/controller.h | 57 ++++++++ processes/click_reader/main.cpp | 143 +++++++++----------- processes/include/system_def.h | 2 + processes/include/util.h | 2 +- processes/udp_server/udp_server.cpp | 9 +- 13 files changed, 254 insertions(+), 106 deletions(-) create mode 100644 processes/click_reader/controller.cpp create mode 100644 processes/click_reader/include/controller.h diff --git a/CMakeLists.txt b/CMakeLists.txt index d369d20..ee6a1dd 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,8 +11,9 @@ SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${GCC_COVERAGE_LINK_FLAGS set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) 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) add_subdirectory(processes) \ No newline at end of file diff --git a/README.md b/README.md index 694d118..8603eaa 100755 --- a/README.md +++ b/README.md @@ -3,28 +3,31 @@ Enable fan pin 20 through raspi-config ``` -sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git ufw python3-picamera2 --no-install-recommends libspdlog-dev +sudo apt install -y cmake build-essential checkinstall zlib1g-dev libssl-dev python3 python3-pip ninja-build git ufw python3-picamera2 libspdlog-dev libeigen3-dev --no-install-recommends ``` -based a bit on example [this is my link](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c) -also want to create mavlink base server using mavproxy +based a bit on [example](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c). + +Using mission planner UDPCI port 14552. Camera is streamed to `http://192.168.0.197:8000/stream.mjpg` ufw for firewall # Used libraries -``` -IMU: -https://github.com/meierman1/IIM42652-Sensor-API - -PRESSURE: -https://github.com/YuechengLi/BMP280_driver (deprecated and not very good, should consider changing to a newer librar, e.g: https://github.com/boschsensortec/BME280_SensorAPI) (check BME280 vs BMP280) -``` + - [BCM2835](https://www.airspayce.com/mikem/bcm2835/index.html), hardware communication. + - [Mavlink](https://mavlink.io/en/). + - [spdlog](https://github.com/gabime/spdlog), the best logging library. + - [Eigen3](https://eigen.tuxfamily.org/dox/index.html), linear algebra. # Modem -just run wvdial +just run wvdial, correct settings should be in `/etc/wvdial.conf`. # TODO - -Fix the spi functions in pressure4. \ No newline at end of file + - EKF for IMU (maybe incorporate GNSS, probably not). + - Yaw, maybe add time-derivates to attitude? + - Fix all unhandled mavlink messages. + - GNSS. + - Start working on xbox controller. + - Temperature. + - Attitude time_boot_ms. diff --git a/build.sh b/build.sh index 0f588ab..1c5da4c 100755 --- a/build.sh +++ b/build.sh @@ -1,4 +1,4 @@ #!/usr/bin/bash cd $(dirname "$0")/build -ninja \ No newline at end of file +ninja -j2 \ No newline at end of file diff --git a/cmake_build.sh b/cmake_build.sh index 1dc1804..52d0e72 100755 --- a/cmake_build.sh +++ b/cmake_build.sh @@ -1,3 +1,4 @@ cd $(dirname "$0")/build -cmake -G Ninja . -ninja \ No newline at end of file +cmake -G Ninja .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j2 + +ninja -j2 \ No newline at end of file diff --git a/processes/click_reader/6dofimu17.c b/processes/click_reader/6dofimu17.c index ff9a660..37ffaf0 100644 --- a/processes/click_reader/6dofimu17.c +++ b/processes/click_reader/6dofimu17.c @@ -304,7 +304,7 @@ err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data ) return err_flag; } -err_t c6dofimu17_get_temperature ( float *temperature ) +err_t c6dofimu17_get_temperature ( double *temperature ) { uint8_t rx_buf[ 2 ]; int16_t tmp; @@ -315,7 +315,7 @@ err_t c6dofimu17_get_temperature ( float *temperature ) tmp <<= 8; tmp |= rx_buf[ 1 ]; - *temperature = ( float ) tmp; + *temperature = ( double ) tmp; *temperature /= 132.48; *temperature += 25; diff --git a/processes/click_reader/CMakeLists.txt b/processes/click_reader/CMakeLists.txt index 5b70712..0bf3a99 100644 --- a/processes/click_reader/CMakeLists.txt +++ b/processes/click_reader/CMakeLists.txt @@ -1,12 +1,14 @@ 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 ${BCM2835_LIB}) diff --git a/processes/click_reader/controller.cpp b/processes/click_reader/controller.cpp new file mode 100644 index 0000000..1ccbadc --- /dev/null +++ b/processes/click_reader/controller.cpp @@ -0,0 +1,100 @@ +#include "controller.h" + +#include + +static vec2d pqr_to_euler(const vec3d&, const vec2d&); +static vec2d accel_to_pitch_roll(const vec3d&); + +vec3d struct_to_vec_scaled(const c6dofimu17_axis_t& data, const double scale) +{ + double data_x = data.x * scale; + double data_y = data.y * scale; + double data_z = data.z * 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()} +{} + +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) +{ + // Crude check for new data + if (last_accel_meas == accel_meas) + { + logger->warn("Last accel measurement indentical to new."); + } + if (last_gyro_meas == gyro_meas) + { + logger->warn("Last gyro measurement identical to new."); + } + last_accel_meas = accel_meas; + last_gyro_meas = gyro_meas; + + auto accel_est = accel_to_pitch_roll(accel_meas); + auto gyro_est = pqr_to_euler(gyro_meas, current_state); + + current_state = (alpha * accel_est) + (1 - alpha) * (current_state + (dt_sec * gyro_est)); +} + + +/* +static vec3d accel_to_g(const c6dofimu17_axis_t &data, const unsigned int g_range) +{ + double tmp = 32768 / g_range; + double accel_x = data.x / tmp; + double accel_y = data.y / tmp; + double accel_z = data.z / tmp; + + accel_x *= GRAVITY; // Convert to m/s^2 + accel_y *= GRAVITY; // Convert to m/s^2 + accel_z *= GRAVITY; // Convert to m/s^2 + + return vec3d{accel_x, accel_y, accel_z}; +} + +static vec3d gyro_to_rad(const c6dofimu17_axis_t &data, const unsigned int dps_range) +{ + double tmp = 32768 / dps_range; + double gyro_x = data.x / tmp; + double gyro_y = data.y / tmp; + double gyro_z = data.z / tmp; + + gyro_x *= M_PI / 180.0; // Convert to radians + gyro_y *= M_PI / 180.0; // Convert to radians + gyro_z *= M_PI / 180.0; // Convert to radians + + return vec3d{gyro_x, gyro_y, gyro_z}; +} +*/ +static vec2d pqr_to_euler(const vec3d& pqr, const vec2d& pitch_roll) +{ + double phi = pitch_roll[0]; + double theta = pitch_roll[1]; + + auto euler_angles = Eigen::MatrixXd{ + {1, sin(phi) * tan(theta), cos(phi) * tan(theta)}, + {0, cos(phi), -sin(phi)} + }; + + return euler_angles * pqr; +} + +static vec2d accel_to_pitch_roll(const vec3d& accel) +{ + return vec2d{ + 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/click_reader/include/6dofimu17.h b/processes/click_reader/include/6dofimu17.h index 51802c4..6ed01da 100644 --- a/processes/click_reader/include/6dofimu17.h +++ b/processes/click_reader/include/6dofimu17.h @@ -313,7 +313,7 @@ err_t c6dofimu17_get_accel_data ( c6dofimu17_axis_t *accel_data ); err_t c6dofimu17_get_gyro_data ( c6dofimu17_axis_t *gyro_data ); -err_t c6dofimu17_get_temperature ( float *temperature ); +err_t c6dofimu17_get_temperature ( double *temperature ); #ifdef __cplusplus } diff --git a/processes/click_reader/include/controller.h b/processes/click_reader/include/controller.h new file mode 100644 index 0000000..389c689 --- /dev/null +++ b/processes/click_reader/include/controller.h @@ -0,0 +1,57 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +// Standard +#include +#include + +// Libs +#include +#include +#include + +// Local +#include "6dofimu17.h" + +#define GRAVITY 9.82f + +typedef Eigen::Vector3d vec3d; +typedef Eigen::Vector2d vec2d; + +vec3d struct_to_vec_scaled(const c6dofimu17_axis_t&, const double); + +class Complementary_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); + + const vec2d get_state(); + +private: + std::shared_ptr logger; + const double alpha; + vec2d current_state; + vec3d last_gyro_meas; + vec3d last_accel_meas; + +}; + +class EKF +{ +public: + EKF(std::shared_ptr); + + void update(const vec3d&, const vec3d&, + double); + + const vec2d get_state(); + +private: + std::shared_ptr logger; + vec2d current_state; + +}; +#endif \ No newline at end of file diff --git a/processes/click_reader/main.cpp b/processes/click_reader/main.cpp index e706f9a..87b32d0 100644 --- a/processes/click_reader/main.cpp +++ b/processes/click_reader/main.cpp @@ -3,6 +3,7 @@ #include #include #include +#include // Pipes #include @@ -21,6 +22,8 @@ #include "pipe_def.h" #include "util.h" #include "6dofimu17.h" +#include "controller.h" +#include "system_def.h" static std::shared_ptr logger; @@ -28,8 +31,6 @@ 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 float accel_to_g(int, unsigned int = 16); -static float gyro_to_dps(int, unsigned int = 1000); int main(int argc, char* argv[]) { @@ -40,7 +41,7 @@ int main(int argc, char* argv[]) logger->debug("Opening sendpipe for writing."); pipe_fd_t sendpipe_fd = open(SEND_PIPE, O_WRONLY); - logger->info("Initializing BCM2835 and SPI!"); + logger->info("Initializing BCM2835 and SPI."); if (spi_init() != OK) { logger->error("spi_init() failed. Are you running as root?"); @@ -50,83 +51,78 @@ int main(int argc, char* argv[]) logger->debug("Initializing 6DOF IMU 17 Click."); c6dofimu17_init_comm(*spi_read_alias, *spi_write_alias); - auto gyro_cfg = std::make_unique(); - auto accel_cfg = std::make_unique(); - - 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.get(), accel_cfg.get()); - - - /* - mavlink_message_t message; - const uint8_t system_id = 42; - const uint8_t base_mode = 0; - const uint8_t custom_mode = 0; - mavlink_msg_global_position_int_pack_chan( - system_id, - MAV_COMP_ID_PERIPHERAL, - MAVLINK_COMM_0, - &message, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9); - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - const int len = mavlink_msg_to_send_buffer(buffer, &message); - - */ - - auto accel_data = std::make_unique(); - auto gyro_data = std::make_unique(); - struct { - float x = 0.0; - float y = 0.0; - float z = 0.0; - } angle; + 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; + + 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(); - float time_d_ms = 0.0; + double time_d = 0.0; + mavlink_message_t msg; + mavlink_attitude_t attitude; // Control loop while (true) { - THREAD_SLEEP(100ms); - int res = 0; - res = c6dofimu17_get_gyro_data(gyro_data.get()); + THREAD_SLEEP(1ms); + + res = c6dofimu17_get_gyro_data(&gyro_data); t_end = std::chrono::high_resolution_clock::now(); - time_d_ms = std::chrono::duration(t_end-t_start).count(); + 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); + if (res != OK) { - logger->warn("Failed to get temperature: {}", res); + logger->warn("Failed to get accel data: {}", res); continue; } - // integrate gyro data - angle.x += gyro_to_dps(gyro_data->x) * time_d_ms / 1000; - angle.y += gyro_to_dps(gyro_data->y) * time_d_ms / 1000; - angle.z += gyro_to_dps(gyro_data->z) * time_d_ms / 1000; - - logger->debug("time_d: {}. ANGLE: X {}, Y {}, Z {}", time_d_ms, angle.x, angle.y, angle.z); - logger->debug("time_d: {}. GYRO: X {}, Y {}, Z {}", time_d_ms, gyro_to_dps(gyro_data->x), gyro_to_dps(gyro_data->y), gyro_to_dps(gyro_data->z)); + 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 attitude into MAVLINK package + attitude.time_boot_ms = 0; + 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; + + mavlink_msg_attitude_encode( + MAV_SYS_ID, + MAV_COMP_ID, + &msg, + &attitude + ); + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + const int len = mavlink_msg_to_send_buffer(buffer, &msg); + + write(sendpipe_fd, buffer, len); } close(sendpipe_fd); @@ -172,22 +168,9 @@ static err_t spi_init() 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_32768); // 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; } - -static float accel_to_g(int data, unsigned int g_range) -{ - float tmp = 32768 / g_range; - tmp = data / tmp; - return tmp; -} -static float gyro_to_dps(int data, unsigned int dps_range) -{ - float tmp = 32768 / dps_range; - tmp = data / tmp; - return tmp; -} diff --git a/processes/include/system_def.h b/processes/include/system_def.h index ad9a96f..b659bdd 100644 --- a/processes/include/system_def.h +++ b/processes/include/system_def.h @@ -4,5 +4,7 @@ #include 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; #endif \ No newline at end of file diff --git a/processes/include/util.h b/processes/include/util.h index ee2923a..1102a34 100644 --- a/processes/include/util.h +++ b/processes/include/util.h @@ -4,7 +4,7 @@ #include #include -#define UDP_LOGLEVEL spdlog::level::debug +#define UDP_LOGLEVEL spdlog::level::info #define CLICK_LOGLEVEL spdlog::level::debug #define CONTROLLER_LOGLEVEL spdlog::level::debug diff --git a/processes/udp_server/udp_server.cpp b/processes/udp_server/udp_server.cpp index 2053b19..04a3d06 100644 --- a/processes/udp_server/udp_server.cpp +++ b/processes/udp_server/udp_server.cpp @@ -64,7 +64,7 @@ static void handle_request_autopilot_capabilities(const mavlink_command_long_t& mavlink_msg_autopilot_version_encode( MAV_SYS_ID, - MAV_COMP_ID_AUTOPILOT1, + MAV_COMP_ID, &response_msg, &version_struct ); @@ -114,10 +114,9 @@ static void recv_thread() static void heartbeat_thread() { mavlink_message_t message; - mavlink_msg_heartbeat_pack_chan( - 1, // System ID - MAV_COMP_ID_AUTOPILOT1, - MAVLINK_COMM_0, + mavlink_msg_heartbeat_pack( + MAV_SYS_ID, // System ID + MAV_COMP_ID, &message, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, -- 2.30.2