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

compiling with shit includes, should fix

This commit is contained in:
franchioping 2026-04-06 03:39:08 +01:00
parent bd81652515
commit a939e01255
11 changed files with 106 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
main/drone.cpp
View File

@ -94,11 +94,11 @@ void drone_controller_task(void *params) {
Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero();
while (true) {
if (xSemaphoreTake(imu_state_mutex, 1)) {
if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) {
imu_state_local = imu_state_var;
xSemaphoreGive(imu_state_mutex);
}
if (xSemaphoreTake(sens_fus_mutex, 1)) {
if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
position_local = sens_fus.position;
velocity_local = sens_fus.velocity;
xSemaphoreGive(sens_fus_mutex);
@ -165,6 +165,7 @@ void motor_throttles_task(void *params) {
}
}
uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
while (true) {
for (int i = 0; i < 4; i++) {
if (!killswitch_active) {

10
main/env_sens.cpp
View File

@ -4,6 +4,16 @@
#include <SPI.h>
#include <Wire.h>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
#include "sens_fus.h"

10
main/gps.cpp
View File

@ -14,6 +14,7 @@ void gps_poll_task(void *_) {
while (true) {
bool has_new_data = false;
bool available = false;
Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
std::optional<Eigen::Vector3f> local_coords;
@ -21,6 +22,7 @@ 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();
}
@ -31,18 +33,20 @@ void gps_poll_task(void *_) {
ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
}
if (has_new_data && sens_fus_mutex) {
if (has_new_data && available && sens_fus_mutex) {
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.measure_gps(
1.0f, local_coords.value(),
Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
ESP_LOGI(TAG, "applied gps measure to sens_fus");
xSemaphoreGive(sens_fus_mutex);
} else {
ESP_LOGD(TAG, "Sens_fus busy, skipping GPS update.");
ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
}
}
vTaskDelay(pdMS_TO_TICKS(50)); // 10Hz polling
vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling
}
}

15
main/gps.h
View File

@ -83,15 +83,12 @@ struct GPS {
}
void poll() {
while (this->gps->read()) {
if (this->gps->newNMEAreceived()) {
char *line = this->gps->lastNMEA();
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
if (!this->gps->parse(line)) {
continue;
}
}
}
// char c = this->gps->read();
// if (this->gps->newNMEAreceived()) {
// char *line = this->gps->lastNMEA();
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
// this->gps->parse(line);
// }
}
};

19
main/imu.cpp
View File

@ -1,7 +1,6 @@
#include "imu.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/idf_additions.h"
#include "hal/spi_types.h"
#include "sens_fus.h"
@ -15,10 +14,13 @@
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
static const char *TAG = "IMU";
void setup_imu() {
imu_state *local_state = new imu_state;
imu_state_mutex = xSemaphoreCreateMutex();
BNO08x *imu = new BNO08x(
bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25,
@ -33,10 +35,12 @@ void setup_imu() {
imu->rpt.rv_game.enable(2500UL);
imu->rpt.linear_accelerometer.enable(2500UL);
imu->rpt.cal_gyro.enable(1000UL);
imu->rpt.cal_gyro.enable(2500UL);
imu->register_cb([imu, local_state]() {
bool needs_updating = false;
if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
return;
if (imu->rpt.rv_game.has_new_data()) {
@ -71,9 +75,16 @@ void setup_imu() {
dcont::Vec3C accel_global =
dcont::apply_rot(&local_state->accel, &local_state->rot);
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) {
ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x,
accel_global.y, accel_global.z, dt);
sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
accel_global.z));
ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(),
sens_fus.velocity.z(), sens_fus.velocity.y());
xSemaphoreGive(sens_fus_mutex);
} else {
ESP_LOGE(TAG, "Failed to get sens_fus mutex.");
@ -83,7 +94,7 @@ void setup_imu() {
}
if (needs_updating) {
if (xSemaphoreTake(imu_state_mutex, 2)) {
if (xSemaphoreTake(imu_state_mutex, 0)) {
imu_state_var = *local_state;
xSemaphoreGive(imu_state_mutex);
}

40
main/main.cpp
View File

@ -1,5 +1,14 @@
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include "Eigen/Core"
#include "driver/gpio.h"
#include "drone.h"
#include "drone_comms.h"
@ -9,7 +18,6 @@
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include <Arduino.h>
#include <cstddef>
#include <cstdint>
#include <optional>
@ -33,8 +41,6 @@ extern "C" void app_main(void) {
gpio_install_isr_service(0);
Serial.begin(115200);
setup_imu();
xTaskCreatePinnedToCore(radio_task, // Function name
"radio_rxtx", // Name for debugging
4096, // Stack size in bytes
@ -43,11 +49,11 @@ extern "C" void app_main(void) {
NULL, // Task handle
0 // Core ID
);
//
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
//
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
//
xTaskCreatePinnedToCore(drone_controller_task, // Function name
"drone_controller_task", // Name for debugging
1024 * 32, // Stack size in bytes
@ -57,28 +63,32 @@ 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
20, // 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
// 30, // Priority (higher = more urgent)
// NULL, // Task handle
// 1 // Core ID
// );
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
setup_imu();
Eigen::Vector3f local_pos = {0, 0, 0};
Eigen::Vector3f local_vel = {0, 0, 0};
bool nav_data_ready = false;
uint64_t last_print_time = 0;
while (true) {
while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
while (packet_tx_queue &&
xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
handle_packet(&packet_data[0]);
}
if (millis() > last_print_time + 1000) {
last_print_time = millis();
std::optional<Eigen::Vector3f> coords;
float lat, lon, alt;

13
main/nav.h
View File

@ -1,12 +1,5 @@
#pragma once
#include "Eigen/Core"
#include "freertos/idf_additions.h"
#include "gps.h"
#include <cmath>
#include <cstdint>
#include <optional>
#ifdef PS
#undef PS
#endif
@ -17,6 +10,12 @@
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
#include "gps.h"
#include <cmath>
#include <cstdint>
#include <optional>
#define WAYPOINT_COUNT 8
struct waypoint {

11
main/packet_handler.cpp
View File

@ -1,6 +1,14 @@
#include "packet_handler.h"
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include "Eigen/Core"
#include "drone.h"
#include "drone_comms.h"
#include "drone_controller.h"
@ -8,6 +16,7 @@
#include "freertos/idf_additions.h"
#include "gps.h"
#include "nav.h"
#include "packet_handler.h"
#include "portmacro.h"
#include "radio.h"
#include "sens_fus.h"

9
main/packet_handler.h
View File

@ -2,8 +2,17 @@
#include <cstdint>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include "drone_comms.h"
#include "freertos/idf_additions.h"
#include <Eigen/Dense>
void handle_packet(uint8_t *packet_addr);

13
main/radio.cpp
View File

@ -1,9 +1,20 @@
#include "radio.h"
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "Esp.h"
#include "esp32-hal-gpio.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include <Arduino.h>
#include "packet_handler.h"
#include <RFM69.h>
#include <SPI.h>
#include <cstdint>

6
main/sens_fus.h
View File

@ -1,5 +1,4 @@
#pragma once
#include "freertos/idf_additions.h"
#include <cmath>
#ifdef PS
@ -12,6 +11,8 @@
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
inline float getYawDifference(const Eigen::Vector3f &v_gps,
const Eigen::Vector3f &v_imu) {
float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
@ -41,8 +42,7 @@ struct sens_fus_compl {
float tau_yaw = 10.0f; // Yaw remains a scalar
Eigen::Matrix3f
yaw_rotation_matrix; // Pre-compute this when yaw_offset changes
Eigen::Matrix3f yaw_rotation_matrix;
void update_yaw_matrix() {
yaw_rotation_matrix =