From 70a1d7d3dd75ede1a2411594e68b2bf876227402 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Nils=20Forss=C3=A9n?= Date: Thu, 19 Jun 2025 11:19:23 +0200 Subject: [PATCH] reduced mutex duration --- README.md | 2 +- processes/controller/controller.cpp | 14 -------------- processes/controller/include/controller.h | 3 --- processes/controller/main.cpp | 13 ++++++++++--- 4 files changed, 11 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 532b720..df00a1a 100755 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ 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 libspdlog-dev libeigen3-dev --no-install-recommends +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 libx264-dev libjpeg-dev --no-install-recommends ``` based a bit on [example](https://github.com/mavlink/mavlink/blob/master/examples/c/udp_example.c). diff --git a/processes/controller/controller.cpp b/processes/controller/controller.cpp index 79ff0f2..827b0de 100644 --- a/processes/controller/controller.cpp +++ b/processes/controller/controller.cpp @@ -36,20 +36,6 @@ const vec2d Complementary_filter::get_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); diff --git a/processes/controller/include/controller.h b/processes/controller/include/controller.h index 8cfc66b..2ec06ea 100644 --- a/processes/controller/include/controller.h +++ b/processes/controller/include/controller.h @@ -29,9 +29,6 @@ private: std::shared_ptr logger; const double alpha; vec2d current_state; - vec3d last_gyro_meas; - vec3d last_accel_meas; - }; class EKF diff --git a/processes/controller/main.cpp b/processes/controller/main.cpp index c0f062e..877b5b2 100644 --- a/processes/controller/main.cpp +++ b/processes/controller/main.cpp @@ -94,6 +94,7 @@ int main(int argc, char* argv[]) { THREAD_SLEEP(CLICK_READ_SPI_PERIOD); + /* __________________________________________________________ */ std::lock_guard lk{data_lock}; logger->debug("Controlling!"); @@ -115,6 +116,8 @@ int main(int argc, char* argv[]) 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); @@ -176,10 +179,11 @@ static err_t spi_init() while (true) { THREAD_SLEEP(CONTROLLER_MAVLINK_PERIOD); - std::lock_guard lk{data_lock}; - logger->debug("Sending IMU Mavlink."); - + + /* __________________________________________________________ */ + data_lock.lock(); + // Pack data into MAVLINK packages attitude.time_boot_ms = system_uptime_ms(); attitude.roll = comp_filter.get_state()[1]; @@ -201,6 +205,9 @@ static err_t spi_init() scaled_imu.ymag = INT16_MAX; scaled_imu.zmag = INT16_MAX; + data_lock.unlock(); + /* __________________________________________________________ */ + mavlink_msg_attitude_encode( MAV_SYS_ID, MAV_COMP_ID, -- 2.30.2