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

i dont even know atp, everything is kinda working lwk just polishing and fixing nav/sens fusion. TODO: Automatic stabilization and landing sequence

This commit is contained in:
franchioping 2026-04-14 16:00:52 +01:00
parent 98f218629e
commit a64795bce0
10 changed files with 135 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_comms

@ -1 +1 @@
Subproject commit 1fee7c4e5d9367a90a259a0ecbc751ae5f53e9b3
Subproject commit 350c761b9fe11f551e5c8ddf11e84252d2f496b4

89
main/drone.cpp
View File

@ -8,6 +8,7 @@
#include "dshot_definitions.h"
#include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
@ -20,6 +21,7 @@
#include <cstdint>
#include <cstring>
#include <optional>
#include <stdlib.h>
#define CONTROLLER_TASK_FREQUENCY 1000.0;
@ -31,45 +33,43 @@ dcont::ControllerConfig default_config() {
// Position Loop (Position -> Velocity)
config.stack.position_pid = {
{1.0f, 1.0f, 1.0f}, // kp
{0.0f, 0.0f, 0.0f}, // ki
{0.0f, 0.0f, 0.0f}, // kd
5.0f // frequency (Hz)
.kp = {1.0f, 1.0f, 1.0f}, // kp
.ki = {0.0f, 0.0f, 0.0f}, // ki
.kd = {0.0f, 0.0f, 0.0f}, // kd
.frequency = 25.0f // frequency (Hz)
};
// Velocity Loop (Velocity -> Acceleration/Tilt)
config.stack.linvel_pid = {
{1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 15.0f};
// Rotation Loop (Angle -> Angular Rate)
config.stack.rotation_pid = {
{4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 50.0f};
// Velocity Loop (Velocity -> Acceleration/Rotation)
config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
.ki = {0.0f, 0.0f, 0.0f},
.kd = {0.0f, 0.0f, 0.0f},
.frequency = 50.0f};
// Rotation Loop (Rotation/Accel -> Angular Rate)
config.stack.rotation_pid = {.kp = {4.0f, 4.0f, 4.0f},
.ki = {1.0f, 1.0f, 1.0f},
.kd = {0.0f, 0.0f, 0.0f},
.frequency = 200.0f};
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
config.stack.rate_pid = {{0.1f, 0.1f, 1.0f},
{0.01f, 0.01f, 0.01f},
{0.001f, 0.001f, 0.001f},
100.0f};
config.stack.rate_pid = {.kp = {0.1f, 0.1f, 1.0f},
.ki = {0.01f, 0.01f, 0.01f},
.kd = {0.001f, 0.001f, 0.001f},
.frequency = 1000.0f};
// 2. Set Constraints
config.stack.max_rate = 3.14f; // ~180 degrees/s
config.stack.max_linvel = 10.0f; // 10 m/s
config.stack.max_linvel = 3.0f; // 10 m/s
// 3. Physical Drone Properties
config.mass = 0.350f; // kg
config.max_thrust = 2.6f; // Newtons
config.max_torque = 0.5f; // Nm
/*
* roll, pitch, yaw
{1.0f, -1.0f, 1.0f}, // Front Right
{-1.0f, -1.0f, -1.0f}, // Front Left
{-1.0f, 1.0f, 1.0f}, // Rear Left
{1.0f, 1.0f, -1.0f} // Rear Right
*/
float mixer[4][3] = {
{1.0f, -1.0f, 1.0f}, // Front Right
{-1.0f, -1.0f, -1.0f}, // Front Left
{-1.0f, 1.0f, 1.0f}, // Rear Left
{1.0f, 1.0f, -1.0f} // Rear Right
// roll, pitch, yaw
{-1.0, -1.0, -1.0}, // Front Left
{1.0, 1.0, -1.0}, // Rear Right
{-1.0, 1.0, 1.0}, // Rear Left
{1.0, -1.0, 1.0}, // Front Right
};
for (int i = 0; i < 4; i++) {
@ -90,9 +90,10 @@ void drone_cont_stabilize() {
// afterwards
}
constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
void drone_controller_task(void *params) {
setup_drone();
uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
imu_state imu_state_local;
Eigen::Vector3f position_local = Eigen::Vector3f::Zero();
@ -118,6 +119,9 @@ void drone_controller_task(void *params) {
packet_controller_input cont_input;
if (current_input_mode == dcont::ModeInput::Acro &&
xSemaphoreTake(controller_input_semaphore, 10)) {
if (millis() - time_last_controller > 1) {
current_controller_input = {0, 0, 0, 0};
}
cont_input = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
@ -136,18 +140,21 @@ void drone_controller_task(void *params) {
} else {
auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero());
dcont::set_input(drone_controller,
dcont::Input{{cont_input.ly, cont_input.lx,
cont_input.rx, cont_input.ry},
{0.0, 0.0, 0.0},
{0.0, 0.0, 0.0},
{0.0, 0.0, 0.0},
{coords.x(), coords.y(), coords.z()},
current_input_mode});
dcont::set_input(
drone_controller,
dcont::Input{.joystick = {.throttle_input = cont_input.ly,
.roll_input = cont_input.rx,
.yaw_input = cont_input.lx,
.pitch_input = cont_input.ry},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {0.0, 0.0, 0.0},
.velocity = {0.0, 0.0, 0.0},
.position = {coords.x(), coords.y(), coords.z()},
.mode = current_input_mode});
}
memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
sizeof(motor_throttles));
// memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
// sizeof(motor_throttles));
vTaskDelay(pdMS_TO_TICKS(wait_ms));
}
@ -156,12 +163,12 @@ void drone_controller_task(void *params) {
const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
GPIO_NUM_15};
// const bool reversed[4] = {false, true, false, false};
DShotRMT *motors[4];
void motor_throttles_task(void *params) {
motor_throttles = (float *)malloc(sizeof(float) * 4);
for (int i = 0; i < 4; i++) {
motor_throttles[i] = 0;
motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
motors[i]->begin();
}
@ -172,13 +179,13 @@ void motor_throttles_task(void *params) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(0);
}
vTaskDelay(1);
vTaskDelay(2);
}
while (true) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f);
}
vTaskDelay(1);
vTaskDelay(2);
}
}

4
main/drone.h
View File

@ -1,5 +1,7 @@
#pragma once
#include "esp32-hal.h"
#include <cstdint>
#ifdef PS
#undef PS
#endif
@ -25,5 +27,5 @@ void drone_controller_task(void *params);
void motor_throttles_task(void *params);
// 3 4 2 1
inline float motor_throttles[4] = {0.01, 0.01, 0.01, 0.01};
inline float *motor_throttles;
inline bool killswitch_active = false;

12
main/env_sens.cpp
View File

@ -48,8 +48,10 @@ void setup() {
}
float get_temperature() {
sensors_event_t temp_event;
if (millis() - env_sens::time_term_read > 20) {
sensors_event_t temp_event;
if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
bme_temp->getEvent(&temp_event);
@ -57,11 +59,15 @@ float get_temperature() {
}
return temp_event.temperature;
}
return env_sens::term_read;
}
float get_pressure() {
sensors_event_t e;
if (millis() - env_sens::time_baro_read > 20) {
sensors_event_t e;
if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
bme_pressure->getEvent(&e);
@ -69,6 +75,8 @@ float get_pressure() {
}
return e.pressure;
}
return env_sens::baro_read;
}
float calculateAltitude(float pressure, float seaLevelPressure,

7
main/env_sens.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <cstdint>
#ifdef PS
#undef PS
#endif
@ -24,6 +25,12 @@ void dbg_sens();
float get_altitude();
void baro_poll_task(void *_);
inline uint64_t time_term_read = 0;
inline uint64_t time_baro_read = 0;
inline float term_read = 0;
inline float baro_read = 0;
} // namespace env_sens
inline SemaphoreHandle_t baro_mutex = NULL;

15
main/imu.cpp
View File

@ -36,10 +36,12 @@ void setup_imu() {
new BNO08x(bno08x_config_t(SPI2_HOST, IMU_MOSI, IMU_MISO, IMU_SCLK,
IMU_CS, IMU_INT, IMU_RST, 2000000, false));
ESP_LOGI(TAG, "INITIALIZING IMU");
if (!imu->initialize()) {
ESP_LOGE(TAG, "BNO08x Init failure.");
ESP.restart();
}
imu->print_product_ids();
imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all);
@ -49,6 +51,7 @@ void setup_imu() {
imu->rpt.cal_gyro.enable(2500UL);
imu->register_cb([imu, local_state]() {
ESP_LOGI("IMU", "CALLBACK");
bool needs_updating = false;
if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
return;
@ -59,8 +62,12 @@ void setup_imu() {
auto sens_rot = imu->rpt.rv_game.get_quat();
auto sens_euler = imu->rpt.rv_game.get_euler();
local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
local_state->rot_euler = {sens_euler.x, sens_euler.y, sens_euler.z};
local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k,
sens_rot.real}; // FIXME: WRONG ROTATION
local_state->rot_euler = {sens_euler.x, -sens_euler.y, -sens_euler.z};
ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f",
local_state->rot_euler.x(), local_state->rot_euler.y(),
local_state->rot_euler.z());
}
if (imu->rpt.cal_gyro.has_new_data()) {
@ -68,7 +75,7 @@ void setup_imu() {
needs_updating = true;
auto cal_gyro = imu->rpt.cal_gyro.get();
local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z};
local_state->angvel = {cal_gyro.x, -cal_gyro.y, -cal_gyro.z};
}
if (imu->rpt.linear_accelerometer.has_new_data()) {
@ -76,7 +83,7 @@ void setup_imu() {
needs_updating = true;
auto sens_accel = imu->rpt.linear_accelerometer.get();
local_state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
local_state->accel = {sens_accel.x, -sens_accel.y, -sens_accel.z};
int64_t current_time = esp_timer_get_time();
if (local_state->last_time != -1) {

12
main/main.cpp
View File

@ -63,21 +63,27 @@ extern "C" void app_main(void) {
1 // Core ID
);
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
// setup_imu();
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
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;
uint64_t last_broadcast_time = 0;
while (true) {
while (packet_tx_queue &&
xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
handle_packet(&packet_data[0]);
}
if (millis() > last_broadcast_time + 100) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
last_broadcast_time = millis();
}
if (millis() > last_print_time + 5000) {
last_print_time = millis();
@ -137,6 +143,6 @@ extern "C" void app_main(void) {
current_controller_input.rx, current_controller_input.ry);
}
vTaskDelay(pdMS_TO_TICKS(10));
vTaskDelay(pdMS_TO_TICKS(50));
}
}

29
main/packet_handler.cpp
View File

@ -3,14 +3,18 @@
#include "drone.h"
#include "drone_comms.h"
#include "drone_controller.h"
#include "env_sens.h"
#include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "gps.h"
#include "imu.h"
#include "nav.h"
#include "portmacro.h"
#include "radio.h"
#include "sens_fus.h"
#include <cstdint>
#include <sys/unistd.h>
#ifdef PS
#undef PS
@ -22,15 +26,15 @@
#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);
packet_command_request *packet =
(packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
send_packet_getter(packet->packet_requested);
}
// NAV SETTERS
@ -64,6 +68,7 @@ void handle_packet(uint8_t *packet_addr) {
if (xSemaphoreTake(controller_input_semaphore, 10)) {
current_controller_input = *packet;
time_last_controller = millis();
xSemaphoreGive(controller_input_semaphore);
}
}
@ -94,23 +99,23 @@ void handle_nav_update(uint8_t *packet_addr) {
}
}
void handle_request(uint8_t *packet_addr) {
void send_packet_getter(PACKET_TYPE requested_type) {
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,
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
{sens_fus.position.x(), sens_fus.position.y(),
.trans = {sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()},
{sens_fus.velocity.x(), sens_fus.velocity.y(),
.vel = {sens_fus.velocity.x(), sens_fus.velocity.y(),
sens_fus.velocity.z()},
{0.0, 0.0, 0.0}});
.rot = {imu_state_var.rot_euler.x(), imu_state_var.rot_euler.y(),
imu_state_var.rot_euler.z()},
.pressure = env_sens::get_pressure(),
.temperature = env_sens::get_temperature()});
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {

5
main/packet_handler.h
View File

@ -3,9 +3,14 @@
#include <cstdint>
#include "drone_comms.h"
#include "esp32-hal.h"
#include "freertos/idf_additions.h"
void handle_packet(uint8_t *packet_addr);
void send_packet_getter(PACKET_TYPE requested_type);
inline SemaphoreHandle_t controller_input_semaphore = NULL;
inline packet_controller_input current_controller_input;
inline uint64_t time_last_controller = 0;

11
main/sens_fus.h
View File

@ -1,4 +1,5 @@
#pragma once
#include "Eigen/Core"
#include <cmath>
#ifdef PS
@ -25,6 +26,7 @@ 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;
@ -75,7 +77,12 @@ struct sens_fus_compl {
// 2. Yaw Correction (only if moving > 1.0 m/s)
if (gps_vel.norm() > 1.0f) {
float yaw_delta = getYawDifference(gps_vel, this->velocity);
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 =
@ -88,6 +95,8 @@ struct sens_fus_compl {
} else if (this->velocity.norm() > 1.0) {
this->velocity *= 1 - (0.90 * dt);
}
this->velocity_last_gps_measurment = this->velocity;
}
void measure_baro(float dt, Eigen::Vector3f baro_pos,