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

sens fus filtering worked WITHOUT motor vibration

This commit is contained in:
franchioping 2026-04-18 01:29:08 +01:00
parent 88d605ae68
commit a4fbd9e8a3
9 changed files with 116 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_comms

@ -1 +1 @@
Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2
Subproject commit d58740e83368c9f2cc2b641ff729d04e7033d007

4
main/env_sens.cpp
View File

@ -20,7 +20,7 @@ Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
static const constexpr char *TAG = "BARO";
#define BARO_SDA GPIO_NUM_47
#define BARO_SDA GPIO_NUM_47 // SDI
#define BARO_SCL GPIO_NUM_48
namespace env_sens {
@ -141,7 +141,7 @@ void baro_poll_task(void *_) {
last_time = now;
}
// BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal
// BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
vTaskDelay(pdMS_TO_TICKS(50));
}
}

33
main/gps.cpp
View File

@ -2,6 +2,7 @@
#include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "sens_fus.h"
#include <cstdint>
@ -16,7 +17,6 @@ void gps_poll_task(void *_) {
uint64_t last_time = millis();
while (true) {
bool has_new_data = false;
bool available = false;
Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
std::optional<Eigen::Vector3f> local_coords;
@ -25,30 +25,37 @@ void gps_poll_task(void *_) {
gps->poll();
if (gps->gps_avaliable()) {
available = true;
local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
local_coords = gps->get_coordinates();
available = local_coords.has_value();
}
xSemaphoreGive(gps_mutex);
has_new_data = local_coords.has_value();
} else {
ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
}
if (has_new_data && available && sens_fus_mutex) {
if (sens_fus_mutex) {
if (available) {
uint64_t current_time = millis();
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.measure_gps(
(current_time - last_time) / 1000.0f, local_coords.value(),
Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
uint64_t current_time = millis();
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.measure_gps(
(current_time - last_time) / 1000.0f, local_coords.value(),
Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
xSemaphoreGive(sens_fus_mutex);
last_time = current_time;
xSemaphoreGive(sens_fus_mutex);
last_time = current_time;
} else {
ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
}
} else {
ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.gps_lost();
xSemaphoreGive(sens_fus_mutex);
}
}
}

36
main/gps.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <cstdint>
#ifdef PS
#undef PS
#endif
@ -14,14 +15,15 @@
#include "esp_log.h"
#include <Adafruit_GPS.h> #include <cmath>
#include <ctime>
#include <optional>
const float TO_RAD = M_PI / 180.0f;
const float KNOTS_TO_M_SEC = 0.5144444f;
const float earth_radius = 6371000.0f;
#define GPS_RX_PIN 18
#define GPS_TX_PIN 17
#define GPS_TX_PIN 18
#define GPS_RX_PIN 17
struct GPS {
Adafruit_GPS *gps;
@ -34,8 +36,34 @@ struct GPS {
origin_long = INFINITY;
}
uint64_t unix_timestamp_millis() {
struct tm t;
t.tm_year = (2000 + this->gps->year) - 1900;
t.tm_mon = this->gps->month - 1;
t.tm_mday = this->gps->day;
t.tm_hour = this->gps->hour;
t.tm_min = this->gps->minute;
t.tm_sec = this->gps->seconds;
t.tm_isdst = 0; // GPS is always UTC, so no Daylight Savings
// mktime converts struct tm to time_t (Unix timestamp)
time_t unixTime = mktime(&t);
// Calculate how many milliseconds have passed since the GPS updated its
// data
unsigned long msSinceUpdate = millis() - this->gps->lastDate;
// Return the base timestamp plus the elapsed seconds
return (uint64_t)unixTime * 1000 + msSinceUpdate + gps->milliseconds;
}
uint64_t unix_timestamp_seconds() {
return this->unix_timestamp_millis() / 1000;
}
void begin() {
Serial2.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
Serial2.begin(9600, SERIAL_8N1, GPS_TX_PIN, GPS_RX_PIN);
this->gps = new Adafruit_GPS(&Serial2);
// this->gps->begin(9600);
this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
@ -95,7 +123,7 @@ struct GPS {
}
if (this->gps->newNMEAreceived()) {
char *line = this->gps->lastNMEA();
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
ESP_LOGI("GPS", "NMEA LINE: %s", line);
this->gps->parse(line);
}
}

12
main/imu.cpp
View File

@ -22,13 +22,13 @@
static const char *TAG = "IMU";
#define IMU_CS GPIO_NUM_3
#define IMU_MOSI GPIO_NUM_2 // DI
#define IMU_RST GPIO_NUM_1
#define IMU_CS GPIO_NUM_3 // Green
#define IMU_MOSI GPIO_NUM_2 // DI - White
#define IMU_RST GPIO_NUM_1 // Red
#define IMU_INT GPIO_NUM_4
#define IMU_MISO GPIO_NUM_5 // SDA
#define IMU_SCLK GPIO_NUM_6 // SCL
#define IMU_INT GPIO_NUM_4 // Yellow
#define IMU_MISO GPIO_NUM_5 // SDA - White
#define IMU_SCLK GPIO_NUM_6 // SCL - Green
BNO08x *setup_imu() {
imu_state *local_state = new imu_state;

22
main/main.cpp
View File

@ -33,17 +33,17 @@ extern "C" void app_main(void) {
gpio_install_isr_service(0);
Serial.begin(115200);
xTaskCreatePinnedToCore(radio_task, // Function name
"radio_rxtx", // Name for debugging
4096, // Stack size in bytes
NULL, // Parameters
5, // Priority (higher = more urgent)
NULL, // Task handle
0 // Core ID
);
xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5,
NULL, 0);
// xTaskCreatePinnedToCore(radio_task, // Function name
// "radio_rxtx", // Name for debugging
// 4096, // Stack size in bytes
// NULL, // Parameters
// 5, // Priority (higher = more urgent)
// NULL, // Task handle
// 0 // Core ID
// );
//
// xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL,
// 5, NULL, 0);
xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);

18
main/packet_handler.cpp
View File

@ -106,35 +106,31 @@ void send_packet_getter(PACKET_TYPE requested_type) {
packet_info_drone_position{
.trans = {sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()},
.accel = {imu_state_var.lin_accel.x, imu_state_var.lin_accel.y,
imu_state_var.lin_accel.z},
.accel = {imu_state_var.lin_accel_global.x,
imu_state_var.lin_accel_global.y,
imu_state_var.lin_accel_global.z},
.vel = {local_vel.x(), local_vel.y(), local_vel.z()},
.rot = {imu_state_var.rot.w(), imu_state_var.rot.x(),
imu_state_var.rot.y(), imu_state_var.rot.z()},
.pressure = env_sens::get_pressure(),
.temperature = env_sens::get_temperature()});
.angvel = {imu_state_var.angvel.x(), imu_state_var.angvel.y(),
imu_state_var.angvel.z()}});
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY) == pdTRUE) {
// TODO: Absolute time from GPS instead of 0
if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY)) {
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_STATUS,
packet_info_drone_status{
.origin = {gps->origin_long, gps->origin_lat},
.time_since_boot = millis(),
.unix_timestamp_millis = 0,
.unix_timestamp_millis = gps->unix_timestamp_millis(),
.gps_fix = gps->gps_avaliable()});
ESP_LOGI("STATUS", "Status sent");
xSemaphoreGive(gps_mutex);
}
}
// Navigation
if (requested_type == PACKET_TYPE::DRONE_NAV) {
uint8_t active_mask, current;
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
active_mask = nav_man.get_active_mask();

9
main/radio.cpp
View File

@ -15,13 +15,14 @@
// Right to left on hardware.
#define RFM69_MOSI GPIO_NUM_12
#define RFM69_SCLK GPIO_NUM_11 // SCK
#define RFM69_MISO GPIO_NUM_13
#define RFM69_CS GPIO_NUM_9 // NSS
#define RFM69_INT GPIO_NUM_8 // DI0
#define RFM69_CS GPIO_NUM_9 // NSS
#define RFM69_RST GPIO_NUM_10
#define RFM69_SCLK GPIO_NUM_11 // SCK
#define RFM69_MOSI GPIO_NUM_12
#define RFM69_MISO GPIO_NUM_13
#define FREQUENCY RF69_433MHZ
#define NODEID 1
#define GROUNDID 99

74
main/sens_fus.h
View File

@ -27,9 +27,18 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps,
struct sens_fus_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity_last_gps_measurment = Eigen::Vector3f::Zero();
Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
float yaw_offset = 0.0f;
Eigen::Vector3f velocity_error = Eigen::Vector3f::Zero();
Eigen::Vector3f position_error = Eigen::Vector3f::Zero();
void gps_lost() {
this->position_error = Eigen::Vector3f::Zero();
this->velocity_error = Eigen::Vector3f::Zero();
}
Eigen::Vector3f velocity_error_mult = {40.0f, 40.0f, 0.0f};
Eigen::Vector3f position_error_mult = {40.0f, 40.0f, 0.0f};
/*
* Tau is the time that the filter takes to reach 1-e^(-1) of the difference
@ -37,67 +46,48 @@ struct sens_fus_compl {
* so at t=tau, were 63% of the way there
* at t=3*tau, were 95% of the way there
*/
Eigen::Vector3f tau_gps_pos = {2.0f, 2.0f, INFINITY};
Eigen::Vector3f tau_gps_vel = {INFINITY, INFINITY, INFINITY};
Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, INFINITY};
Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 10.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
float tau_yaw = 10.0f; // Yaw remains a scalar
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 4.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f};
Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
void update_yaw_matrix() {
// yaw_rotation_matrix =
// Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ())
// .toRotationMatrix();
}
void predict(float dt, Eigen::Vector3f accel) {
Eigen::Vector3f accel_rotated = yaw_rotation_matrix * accel;
Eigen::Vector3f accel_err_rmvd =
accel.array() -
this->velocity_error.array() * this->velocity_error_mult.array();
Eigen::Vector3f next_velocity =
this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt;
this->velocity + (this->last_accel_world + accel_err_rmvd) * 0.5f * dt;
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->position =
this->position.array() +
(((this->velocity + next_velocity) * 0.5f).array() -
this->position_error.array() * this->position_error_mult.array()) *
dt;
this->velocity = next_velocity;
this->last_accel_world = accel_rotated;
this->last_accel_world = accel_err_rmvd;
}
void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
// alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
float alpha_yaw = dt / (tau_yaw + dt);
// res = (1 - alpha) * state + alpha * measurement
this->velocity_error = this->velocity - gps_vel;
this->position_error = this->position - gps_pos;
// next_state = (1 - alpha) * state + alpha * measurement
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
alpha_pos.array() * gps_pos.array();
// 2. Yaw Correction (only if moving > 1.0 m/s)
if (gps_vel.norm() > 0.2f) {
Eigen::Vector3f delta_v_imu =
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);
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();
} else if (this->velocity.norm() > 0.2f) {
this->velocity *= 1 - (0.90 * dt);
}
this->velocity_last_gps_measurment = this->velocity;
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * gps_vel.array();
}
void measure_baro(float dt, Eigen::Vector3f baro_pos,