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

change nav.h to sens_fus.h; create nav.h to manage waypoints (automatic navigation); add packet_handler.h

This commit is contained in:
franchioping 2026-04-03 17:52:02 +01:00
parent aa2a3710d0
commit 76db0b0d15
9 changed files with 290 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
main/env_sens.cpp
View File

@ -5,7 +5,7 @@
#include <Wire.h>
#include "freertos/idf_additions.h"
#include "nav.h"
#include "sens_fus.h"
#define SEALEVELPRESSURE_HPA (1030)
@ -90,18 +90,19 @@ void baro_poll_task(void *_) {
float v_z = (filtered_alt - last_alt) / dt;
if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) {
if (sens_fus_mutex &&
xSemaphoreTake(sens_fus_mutex, (TickType_t)20) == pdTRUE) {
Eigen::Vector3f baro_pos = nav_filter.position;
Eigen::Vector3f baro_pos = sens_fus.position;
baro_pos.z() = filtered_alt;
Eigen::Vector3f baro_vel = nav_filter.velocity;
Eigen::Vector3f baro_vel = sens_fus.velocity;
baro_vel.z() = v_z;
// Update the filter with Baro data
nav_filter.measure_baro(dt, baro_pos, baro_vel);
sens_fus.measure_baro(dt, baro_pos, baro_vel);
xSemaphoreGive(nav_mutex);
xSemaphoreGive(sens_fus_mutex);
}
last_alt = filtered_alt;

12
main/gps.cpp
View File

@ -1,6 +1,6 @@
#include "gps.h"
#include "esp_log.h"
#include "nav.h"
#include "sens_fus.h"
static const char *TAG = "GPS_TASK";
@ -17,15 +17,15 @@ void gps_poll_task(void *_) {
// ESP_LOGI(TAG, "Polling gps.");
gps->poll();
if (gps->gps_avaliable() && nav_mutex &&
xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
if (gps->gps_avaliable() && sens_fus_mutex &&
xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
auto coords = gps->get_coordinates();
if (coords.has_value()) {
nav_filter.measure_gps(1.0, gps->get_coordinates().value(),
Eigen::Vector3f(vel.x(), vel.y(), 0.0));
sens_fus.measure_gps(1.0, gps->get_coordinates().value(),
Eigen::Vector3f(vel.x(), vel.y(), 0.0));
}
xSemaphoreGive(nav_mutex);
xSemaphoreGive(sens_fus_mutex);
}
xSemaphoreGive(gps_mutex);
} else {

10
main/imu.cpp
View File

@ -3,7 +3,7 @@
#include "esp_timer.h"
#include "freertos/idf_additions.h"
#include "hal/spi_types.h"
#include "nav.h"
#include "sens_fus.h"
#ifdef PS
#undef PS
@ -50,10 +50,10 @@ BNO08x *setup_imu(imu_state *state) {
float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
Vec3C accel_global = apply_rot(&state->accel, &state->rot);
if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
accel_global.z));
xSemaphoreGive(nav_mutex);
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
accel_global.z));
xSemaphoreGive(sens_fus_mutex);
} else {
ESP_LOGE(TAG, "Failed to get mutex.");
}

61
main/main.cpp
View File

@ -13,18 +13,19 @@
#include "env_sens.h"
#include "gps.h"
#include "nav.h"
#include "radio.h"
#include "imu.h"
#include "nav.h"
#include "packet_handler.h"
#include "radio.h"
#include "sens_fus.h"
static const char *TAG = "MAIN";
void handle_packet(uint8_t *packet_addr);
extern "C" void app_main(void) {
sens_fus_mutex = xSemaphoreCreateMutex();
nav_mutex = xSemaphoreCreateMutex();
initArduino();
gpio_install_isr_service(0);
Serial.begin(115200);
@ -38,7 +39,7 @@ extern "C" void app_main(void) {
NULL, // Parameters
5, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
0 // Core ID
);
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
@ -73,11 +74,12 @@ extern "C" void app_main(void) {
}
env_sens::dbg_sens();
if (nav_mutex && xSemaphoreTake(nav_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
local_pos = nav_filter.position;
local_vel = nav_filter.velocity;
if (sens_fus_mutex &&
xSemaphoreTake(sens_fus_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
local_pos = sens_fus.position;
local_vel = sens_fus.velocity;
nav_data_ready = true;
xSemaphoreGive(nav_mutex); // RELEASE IMMEDIATELY
xSemaphoreGive(sens_fus_mutex);
}
if (nav_data_ready) {
@ -90,42 +92,3 @@ extern "C" void app_main(void) {
vTaskDelay(pdMS_TO_TICKS(1000));
}
}
void handle_packet(uint8_t *packet_addr) {
PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
packet_command_request *packet =
(packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
PACKET_TYPE requested_type = packet->packet_requested;
std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
{nav_filter.position.x(), nav_filter.position.y(),
nav_filter.position.z()},
{nav_filter.velocity.x(), nav_filter.velocity.y(),
nav_filter.velocity.z()},
{0.0, 0.0, 0.0}});
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_STATUS,
packet_info_drone_status{
{gps->origin_long, gps->origin_lat}, millis(), 0});
xSemaphoreGive(gps_mutex);
}
}
if (resp_packet.first != nullptr) {
xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
}
}
}

1
main/nav.cpp Normal file
View File

@ -0,0 +1 @@
#include "nav.h"

98
main/nav.h
View File

@ -1,6 +1,8 @@
#pragma once
#include "Eigen/Core"
#include "freertos/idf_additions.h"
#include <cmath>
#include <cstdint>
#ifdef PS
#undef PS
@ -12,87 +14,33 @@
#include <Eigen/Dense>
inline float getYawDifference(const Eigen::Vector3f &v_gps,
const Eigen::Vector3f &v_imu) {
float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
#define WAYPOINT_COUNT 8
float delta_yaw = yaw_gps - yaw_imu;
struct waypoint {
Eigen::Vector3f coords; // long, lat, alt
bool active; // active or to be skipped
};
return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
}
struct drone_nav {
waypoint waypoints[WAYPOINT_COUNT];
uint8_t current_waypoint;
struct nav_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
float yaw_offset = 0.0f;
// Time Constants per axis (X, Y, Z)
// Lower = faster tracking of GPS; Higher = smoother/more IMU trust
Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
float tau_yaw = 2.0f; // Yaw remains a scalar
void predict(float dt, Eigen::Vector3f accel) {
// Rotate body-frame accel to world-frame
Eigen::Vector3f accel_rotated =
Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
// Trapezoidal integration for position
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
}
void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: 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);
// 1. Position Update (Element-wise LPF)
// res = (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() > 1.0f) {
float yaw_delta = getYawDifference(gps_vel, this->velocity);
this->yaw_offset += yaw_delta * alpha_yaw;
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
void set_active_mask(uint8_t mask) {
for (int i = 0; i < WAYPOINT_COUNT; i++) {
this->waypoints[i].active = (mask & (1 << i)) != 0;
}
// 3. Velocity Update (Element-wise LPF)
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,
Eigen::Vector3f baro_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
alpha_pos.array() * baro_pos.array();
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * baro_vel.array();
uint8_t get_active_mask() {
uint8_t mask = 0;
for (int i = 0; i < WAYPOINT_COUNT; i++) {
if (waypoints[i].active) {
mask |= (1 << i);
}
}
return mask;
}
};
inline SemaphoreHandle_t nav_mutex = NULL;
inline nav_compl nav_filter;
inline drone_nav nav_man;

134
main/packet_handler.cpp Normal file
View File

@ -0,0 +1,134 @@
#include "packet_handler.h"
#include "Eigen/Core"
#include "drone_comms.h"
#include "freertos/idf_additions.h"
#include "gps.h"
#include "nav.h"
#include "portmacro.h"
#include "radio.h"
#include "sens_fus.h"
#include <cstdint>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
void handle_request(uint8_t *packet_addr);
void handle_waypoint_update(uint8_t *packet_addr, uint8_t index);
void handle_nav_update(uint8_t *packet_addr);
void handle_packet(uint8_t *packet_addr) {
PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
handle_request(packet_addr);
}
// NAV SETTERS
if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
uint8_t index = packet_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
handle_waypoint_update(packet_addr, index);
}
if (packet_type == PACKET_TYPE::DRONE_NAV) {
handle_nav_update(packet_addr);
}
}
void handle_waypoint_update(uint8_t *packet_addr, uint8_t index) {
packet_drone_nav_waypoint *packet =
(packet_drone_nav_waypoint *)(packet_addr + sizeof(PACKET_TYPE));
float lon, lat, alt;
lon = packet->coord[0];
lat = packet->coord[1];
alt = packet->coord[2];
Eigen::Vector3f coords(lon, lat, alt);
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
nav_man.waypoints[index].coords = coords;
xSemaphoreGive(nav_mutex);
}
}
void handle_nav_update(uint8_t *packet_addr) {
packet_drone_nav *packet =
(packet_drone_nav *)(packet_addr + sizeof(PACKET_TYPE));
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
nav_man.set_active_mask(packet->active_mask);
nav_man.current_waypoint = packet->current_waypoint;
}
}
void handle_request(uint8_t *packet_addr) {
packet_command_request *packet =
(packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
PACKET_TYPE requested_type = packet->packet_requested;
std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
// TODO: send pitch, roll, yaw
resp_packet =
create_packet_pooled(PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
{sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()},
{sens_fus.velocity.x(), sens_fus.velocity.y(),
sens_fus.velocity.z()},
{0.0, 0.0, 0.0}});
}
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
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_STATUS,
packet_info_drone_status{
{gps->origin_long, gps->origin_lat}, millis(), 0});
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();
current = nav_man.current_waypoint;
xSemaphoreGive(nav_mutex);
resp_packet = create_packet_pooled(
PACKET_TYPE::DRONE_NAV, packet_drone_nav{active_mask, current});
}
}
if (requested_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
requested_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
uint8_t index = requested_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
Eigen::Vector3f coords;
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
coords = nav_man.waypoints[index].coords;
xSemaphoreGive(nav_mutex);
}
resp_packet = create_packet_pooled(
requested_type,
packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}});
}
if (resp_packet.first != nullptr) {
xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
}
}

5
main/packet_handler.h Normal file
View File

@ -0,0 +1,5 @@
#pragma once
#include <cstdint>
void handle_packet(uint8_t *packet_addr);

97
main/sens_fus.h Normal file
View File

@ -0,0 +1,97 @@
#pragma once
#include "freertos/idf_additions.h"
#include <cmath>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
inline float getYawDifference(const Eigen::Vector3f &v_gps,
const Eigen::Vector3f &v_imu) {
float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
float delta_yaw = yaw_gps - yaw_imu;
return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
}
struct sens_fus_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
float yaw_offset = 0.0f;
// Time Constants per axis (X, Y, Z)
// Lower = faster tracking of GPS; Higher = smoother/more IMU trust
Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
float tau_yaw = 2.0f; // Yaw remains a scalar
void predict(float dt, Eigen::Vector3f accel) {
// Rotate body-frame accel to world-frame
Eigen::Vector3f accel_rotated =
Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
// Trapezoidal integration for position
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
}
void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: 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);
// 1. Position Update (Element-wise LPF)
// res = (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() > 1.0f) {
float yaw_delta = getYawDifference(gps_vel, this->velocity);
this->yaw_offset += yaw_delta * alpha_yaw;
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
}
// 3. Velocity Update (Element-wise LPF)
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,
Eigen::Vector3f baro_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
alpha_pos.array() * baro_pos.array();
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * baro_vel.array();
}
};
inline SemaphoreHandle_t sens_fus_mutex = NULL;
inline sens_fus_compl sens_fus;