Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

only update yaw if velocity delta has similar norm.

This commit is contained in:
franchioping 2026-04-14 16:14:04 +01:00
parent a64795bce0
commit eaa2c1d4ad
2 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
main/main.cpp
View File

@ -6,7 +6,7 @@
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h" #include "freertos/idf_additions.h"
#include "freertos/projdefs.h" #include "freertos/projdefs.h"
#include "freertos/task.h" #include < cstddef> #include "freertos/task.h"
#include <cstdint> #include <cstdint>
#include <optional> #include <optional>
@ -54,14 +54,14 @@ extern "C" void app_main(void) {
// 1 // Core ID // 1 // Core ID
// ); // );
xTaskCreatePinnedToCore(motor_throttles_task, // Function name // xTaskCreatePinnedToCore(motor_throttles_task, // Function name
"motor_throttles_task", // Name for debugging // "motor_throttles_task", // Name for debugging
1024 * 4, // Stack size in bytes // 1024 * 4, // Stack size in bytes
NULL, // Parameters // NULL, // Parameters
24, // Priority (higher = more urgent) // 24, // Priority (higher = more urgent)
NULL, // Task handle // NULL, // Task handle
1 // Core ID // 1 // Core ID
); // );
// setup_imu(); // setup_imu();
ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");

16
main/sens_fus.h
View File

@ -1,6 +1,7 @@
#pragma once #pragma once
#include "Eigen/Core" #include "Eigen/Core"
#include <cmath> #include <cmath>
#include <cstdlib>
#ifdef PS #ifdef PS
#undef PS #undef PS
@ -42,7 +43,7 @@ struct sens_fus_compl {
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f}; Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f}; Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f};
float tau_yaw = 20.0f; // Yaw remains a scalar float tau_yaw = 20.0f;
Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval(); Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
@ -81,14 +82,15 @@ struct sens_fus_compl {
this->velocity - this->velocity_last_gps_measurment; this->velocity - this->velocity_last_gps_measurment;
Eigen::Vector3f delta_v_gps = this->velocity - gps_vel; Eigen::Vector3f delta_v_gps = this->velocity - gps_vel;
float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu); if (abs(delta_v_gps.norm() - delta_v_imu.norm()) < 1.0 * dt) {
float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
this->yaw_offset += yaw_delta * alpha_yaw; this->yaw_offset += yaw_delta * alpha_yaw;
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
this->update_yaw_matrix();
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
this->update_yaw_matrix();
}
this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
this->velocity.array() + this->velocity.array() +
alpha_vel.array() * gps_vel.array(); alpha_vel.array() * gps_vel.array();