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

16
main/sens_fus.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "Eigen/Core"
#include <cmath>
#include <cstdlib>
#ifdef PS
#undef PS
@ -42,7 +43,7 @@ struct sens_fus_compl {
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.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();
@ -81,14 +82,15 @@ struct sens_fus_compl {
this->velocity - this->velocity_last_gps_measurment;
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 =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
this->update_yaw_matrix();
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->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
this->velocity.array() +
alpha_vel.array() * gps_vel.array();