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

working nav kinda

This commit is contained in:
franchioping 2026-04-07 10:48:44 +01:00
parent a939e01255
commit 3da916d1aa
13 changed files with 106 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/drone.cpp
View File

@ -148,7 +148,7 @@ void drone_controller_task(void *params) {
}
}
const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC,
const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_NC, GPIO_NUM_NC,
GPIO_NUM_NC};
void motor_throttles_task(void *params) {

11
main/drone.h
View File

@ -1,9 +1,5 @@
#pragma once
#include "Eigen/Core"
#include "drone_comms.h"
#include "drone_controller.h"
#ifdef PS
#undef PS
#endif
@ -14,16 +10,19 @@
#include <Eigen/Dense>
#include "drone_comms.h"
#include "drone_controller.h"
void setup_drone();
inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; }
inline dcont::StackedController *drone_controller = nullptr;
inline dcont::ModeInput current_input_mode;
inline dcont::ModeInput current_input_mode = dcont::ModeInput::Acro;
void drone_controller_task(void *params);
void motor_throttles_task(void *params);
inline float motor_throttles[4];
inline float motor_throttles[4] = {0.5, 0.5, 0.5, 0.5};
inline bool killswitch_active;

16
main/env_sens.cpp
View File

@ -1,21 +1,13 @@
#include "env_sens.h"
#include "sens_fus.h"
#include "esp_log.h"
#include <Adafruit_BME280.h>
#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"
#define SEALEVELPRESSURE_HPA (1030)
@ -34,6 +26,8 @@ void setup() {
if (!bme.begin()) {
ESP_LOGE(TAG, "Couldn't find a valid sensor");
ESP.restart();
return;
}
ESP_LOGI(TAG, "BARO SETUP COMPLETE.");

10
main/env_sens.h
View File

@ -1,5 +1,15 @@
#pragma once
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
namespace env_sens {

13
main/gps.cpp
View File

@ -1,7 +1,9 @@
#include "gps.h"
#include "Eigen/Core"
#include "esp32-hal.h"
#include "esp_log.h"
#include "sens_fus.h"
#include <cstdint>
static const char *TAG = "GPS_TASK";
@ -12,6 +14,7 @@ void gps_poll_task(void *_) {
ESP_LOGI(TAG, "GPS TASK INIT.");
uint64_t last_time = millis();
while (true) {
bool has_new_data = false;
bool available = false;
@ -34,19 +37,21 @@ void gps_poll_task(void *_) {
}
if (has_new_data && available && sens_fus_mutex) {
uint64_t current_time = millis();
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.measure_gps(
1.0f, local_coords.value(),
(current_time - last_time) / 1000.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);
last_time = current_time;
} else {
ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
}
}
vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling
vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling
}
}

18
main/gps.h
View File

@ -56,8 +56,13 @@ struct GPS {
std::optional<Eigen::Vector3f>
waypoint_to_xyz(float latitude, float longitude, float height) {
if (this->origin_lat == INFINITY || this->origin_long == INFINITY) {
if (this->gps_avaliable()) {
this->origin_lat = this->gps->latitudeDegrees;
this->origin_long = this->gps->longitudeDegrees;
} else {
return std::nullopt;
}
}
float dLat = (latitude - this->origin_lat) * TO_RAD;
float dLon = (longitude - this->origin_long) * TO_RAD;
@ -72,8 +77,7 @@ struct GPS {
}
std::optional<Eigen::Vector3f> get_coordinates() {
if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 &&
this->gps->longitudeDegrees == 0.0)) {
if (this->gps->fix == false) {
return std::nullopt;
}
float latitude = this->gps->latitudeDegrees;
@ -83,12 +87,12 @@ struct GPS {
}
void poll() {
// char c = this->gps->read();
// if (this->gps->newNMEAreceived()) {
// char *line = this->gps->lastNMEA();
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);
// }
this->gps->parse(line);
}
}
};

21
main/imu.cpp
View File

@ -1,8 +1,5 @@
#include "imu.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "hal/spi_types.h"
#include "sens_fus.h"
#include "Esp.h"
#ifdef PS
#undef PS
@ -14,6 +11,11 @@
#include <Eigen/Dense>
#include "esp_log.h"
#include "esp_timer.h"
#include "hal/spi_types.h"
#include "sens_fus.h"
#include "freertos/idf_additions.h"
static const char *TAG = "IMU";
@ -28,6 +30,7 @@ void setup_imu() {
if (!imu->initialize()) {
ESP_LOGE(TAG, "BNO08x Init failure.");
ESP.restart();
}
imu->dynamic_calibration_autosave_enable();
@ -75,16 +78,16 @@ void setup_imu() {
dcont::Vec3C accel_global =
dcont::apply_rot(&local_state->accel, &local_state->rot);
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) {
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x,
accel_global.y, accel_global.z, dt);
// 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());
// 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.");

64
main/main.cpp
View File

@ -1,14 +1,3 @@
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include "Eigen/Core"
#include "driver/gpio.h"
#include "drone.h"
#include "drone_comms.h"
@ -17,8 +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 < cstddef>
#include <cstdint>
#include <optional>
@ -35,6 +23,7 @@ static const char *TAG = "MAIN";
extern "C" void app_main(void) {
sens_fus_mutex = xSemaphoreCreateMutex();
configASSERT(sens_fus_mutex);
nav_mutex = xSemaphoreCreateMutex();
initArduino();
@ -54,24 +43,24 @@ extern "C" void app_main(void) {
//
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
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
// xTaskCreatePinnedToCore(drone_controller_task, // Function name
// "drone_controller_task", // Name for debugging
// 1024 * 32, // Stack size in bytes
// NULL, // Parameters
// 30, // Priority (higher = more urgent)
// 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();
@ -92,19 +81,31 @@ extern "C" void app_main(void) {
std::optional<Eigen::Vector3f> coords;
float lat, lon, alt;
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
bool gps_values = false;
bool fix = false;
uint8_t sat_count = 0;
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
if (gps->gps_avaliable()) {
coords = gps->get_coordinates();
lat = gps->gps->latitudeDegrees;
lon = gps->gps->longitudeDegrees;
alt = gps->gps->altitude;
gps_values = true;
}
sat_count = gps->gps->satellites;
fix = gps->gps->fix;
xSemaphoreGive(gps_mutex);
if (gps_values) {
ESP_LOGI(TAG,
"loc -> lat: %f, long: %f, height: %f, sat_c: %d, fix: %b",
lat, lon, alt, sat_count, fix);
}
if (coords.has_value()) {
auto D_pos = coords.value();
ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", lat, lon, alt);
ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
D_pos[2]);
}
@ -125,6 +126,13 @@ extern "C" void app_main(void) {
ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
local_vel[2]);
}
ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0],
motor_throttles[1], motor_throttles[2], motor_throttles[3]);
ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)",
current_controller_input.lx, current_controller_input.ly,
current_controller_input.rx, current_controller_input.ry);
}
vTaskDelay(pdMS_TO_TICKS(10));

11
main/nav.h
View File

@ -1,15 +1,4 @@
#pragma once
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
#include "gps.h"
#include <cmath>

12
main/packet_handler.cpp
View File

@ -1,13 +1,4 @@
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include "Eigen/Core"
#include "packet_handler.h"
#include "drone.h"
#include "drone_comms.h"
@ -16,7 +7,6 @@
#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,17 +2,8 @@
#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);

11
main/radio.cpp
View File

@ -1,15 +1,5 @@
#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"
@ -68,6 +58,7 @@ void radio_task(void *pvParameters) {
radio.setHighPower(true);
radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
} else {
ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
ESP.restart();

21
main/sens_fus.h
View File

@ -34,15 +34,15 @@ 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 = {4.0f, 4.0f, 10.0f};
Eigen::Vector3f tau_gps_vel = {5.0f, 5.0f, INFINITY};
Eigen::Vector3f tau_gps_pos = {20.0f, 20.0f, INFINITY};
Eigen::Vector3f tau_gps_vel = {40.0f, 40.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f};
float tau_yaw = 10.0f; // Yaw remains a scalar
float tau_yaw = 20.0f; // Yaw remains a scalar
Eigen::Matrix3f yaw_rotation_matrix;
Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
void update_yaw_matrix() {
yaw_rotation_matrix =
@ -81,12 +81,13 @@ struct sens_fus_compl {
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
this->update_yaw_matrix();
}
// res = (1 - alpha) * state + alpha * measurement
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
this->velocity.array() +
alpha_vel.array() * gps_vel.array();
} else if (this->velocity.norm() > 1.0) {
this->velocity *= 1 - (0.90 * dt);
}
}
void measure_baro(float dt, Eigen::Vector3f baro_pos,