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

Compare commits

base: Cansat:main
Branches Tags
Cansat:main
Cansat:before_fuckup
Cansat:filter
Cansat:eigen
..
compare: Cansat:eigen
Branches Tags
Cansat:before_fuckup
Cansat:main
Cansat:filter
Cansat:eigen

No commits in common. "main" and "eigen" have entirely different histories.
main ... eigen

27 changed files with 124 additions and 1723 deletions
Show Stats Expand all files Collapse all files

18
.gitmodules vendored
View File

@ -17,21 +17,3 @@
[submodule "components/esp32_Adafruit_Sensor"] [submodule "components/esp32_Adafruit_Sensor"]
path = components/esp32_Adafruit_Sensor path = components/esp32_Adafruit_Sensor
url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_Sensor.git url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_Sensor.git
[submodule "components/DShotRMT"]
path = components/DShotRMT
url = ssh://git@forgejo.galard.uk:222/Cansat/DShotRMT.git
[submodule "components/RFM69_LowPowerLab"]
path = components/RFM69_LowPowerLab
url = ssh://git@forgejo.galard.uk:222/Cansat/RFM69_LowPowerLab.git
[submodule "components/SPIFlash_LowPowerLab"]
path = components/SPIFlash_LowPowerLab
url = ssh://git@forgejo.galard.uk:222/Cansat/SPIFlash_LowPowerLab.git
[submodule "components/drone_comms"]
path = components/drone_comms
url = ssh://git@forgejo.galard.uk:222/Cansat/drone_comms.git

1
components/DShotRMT

@ -1 +0,0 @@
Subproject commit 86d682313234556720dc63ad53aace47ab3d190c

1
components/RFM69_LowPowerLab

@ -1 +0,0 @@
Subproject commit 9b083db90723c4911ac79edb6c1aa35c4c127cb8

1
components/SPIFlash_LowPowerLab

@ -1 +0,0 @@
Subproject commit 60ede5c9393ca7bbcf3ea62a55e26f6234b29f7c

1
components/drone_comms

@ -1 +0,0 @@
Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20

2
components/drone_controller

@ -1 +1 @@
Subproject commit 3504bc479075bbc363697edc4123978e4342c823 Subproject commit 3ad9eb42b9734d72c3f6fbc3d3e107d21a32b589

2
components/esp32_Adafruit_GPS

@ -1 +1 @@
Subproject commit 06ee1eea5a5073337591edc713a755306cea1331 Subproject commit 3df2e79d866d1a46fe79f9094a8647a3fd8c5c40

73
dependencies.lock
View File

@ -154,18 +154,8 @@ dependencies:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 0.6.1~4 version: 0.6.1~4
espressif/dl_fft:
component_hash: 7dadbd644c0d7ba4733cc3726ec4cff6edf27b043725e1115861dec1609a3d28
dependencies:
- name: idf
require: private
version: '>=5.0'
source:
registry_url: https://components.espressif.com
type: service
version: 0.3.1
espressif/esp-dsp: espressif/esp-dsp:
component_hash: 42dce32d46ac93dc11f60d368e29a830e9661c7345d794b8a45c343479cae636 component_hash: 939e9c053487d6e7b7320a5cb761b2200e4b331730d6721668755ef76ab9f067
dependencies: dependencies:
- name: idf - name: idf
require: private require: private
@ -173,7 +163,7 @@ dependencies:
source: source:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 1.7.0 version: 1.7.1
espressif/esp-modbus: espressif/esp-modbus:
component_hash: 5d5e90b9e55721a8a194b301ad8102d4affb647f47b74cd413ff7d1ce2c1169c component_hash: 5d5e90b9e55721a8a194b301ad8102d4affb647f47b74cd413ff7d1ce2c1169c
dependencies: dependencies:
@ -191,24 +181,6 @@ dependencies:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 0.0.11 version: 0.0.11
espressif/esp-sr:
component_hash: c518b25995cf03021a09442feedb7e40acba9f15d6aa903ebdfe0f657b08197b
dependencies:
- name: espressif/dl_fft
registry_url: https://components.espressif.com
require: private
version: '>=0.2.0'
- name: espressif/esp-dsp
registry_url: https://components.espressif.com
require: private
version: 1.7.0
- name: idf
require: private
version: '>=5.0'
source:
registry_url: https://components.espressif.com
type: service
version: 2.4.0
espressif/esp-zboss-lib: espressif/esp-zboss-lib:
component_hash: 321883d142421f65009972408287441794250057668a11abbdfd8bec77c3309a component_hash: 321883d142421f65009972408287441794250057668a11abbdfd8bec77c3309a
dependencies: dependencies:
@ -282,7 +254,7 @@ dependencies:
type: service type: service
version: 1.2.2 version: 1.2.2
espressif/esp_modem: espressif/esp_modem:
component_hash: f4fa6dab2496af2673a68881132bb7ce0f05bf62e30e5bd0d45b607066378a28 component_hash: 07b6ca85dc0c1edbcd76f650005c74067001db138db6bfa106bf05567adca4af
dependencies: dependencies:
- name: idf - name: idf
require: private require: private
@ -290,7 +262,7 @@ dependencies:
source: source:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 2.0.1 version: 2.0.0
espressif/esp_rainmaker: espressif/esp_rainmaker:
component_hash: f6fe458fc7a0102ee2879f0247da4b41419e6c07de12031f66e5e9454d25baaa component_hash: f6fe458fc7a0102ee2879f0247da4b41419e6c07de12031f66e5e9454d25baaa
dependencies: dependencies:
@ -403,8 +375,35 @@ dependencies:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 1.0.3 version: 1.0.3
espressif/lan867x:
component_hash: 651c9d6e3d96acb08bd5066144b12a12e94df0a6241127a0538d8680689de2c4
dependencies:
- name: espressif/lan86xx_common
registry_url: https://components.espressif.com
require: private
version: '*'
- name: idf
require: private
version: '>=5.3'
source:
registry_url: https://components.espressif.com
type: service
targets:
- esp32
- esp32p4
version: 2.0.0
espressif/lan86xx_common:
component_hash: 6dcb1be9f43ae940979bd628fa5bee278949b66c545e1fe2321e486102e81026
dependencies:
- name: idf
require: private
version: '>=5.2'
source:
registry_url: https://components.espressif.com
type: service
version: 1.0.0
espressif/libsodium: espressif/libsodium:
component_hash: b51f5836f044d8b7fbb1784257605c47ff7356f701377b005912fe6a2f12db37 component_hash: ec52dce5da4af52b1908a0e23e2cf8f492edea090450e51f0d1fbe1108b6d58e
dependencies: dependencies:
- name: idf - name: idf
require: private require: private
@ -412,9 +411,9 @@ dependencies:
source: source:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 1.0.21 version: 1.0.20~4
espressif/mdns: espressif/mdns:
component_hash: 1ebe3bd675bb9d1c58f52bc0b609b32f74e572b01c328f9e61282040c775495c component_hash: 7c0fa01a1cd0e72a87ec1928c3b661c0a3a9034a6d3a69dcf4850db8c6f272db
dependencies: dependencies:
- name: idf - name: idf
require: private require: private
@ -422,7 +421,7 @@ dependencies:
source: source:
registry_url: https://components.espressif.com registry_url: https://components.espressif.com
type: service type: service
version: 1.11.0 version: 1.10.1
espressif/network_provisioning: espressif/network_provisioning:
component_hash: ef2e10182fd1861e68b821491916327c25416ca7ae70e5a6e43313dbc71fe993 component_hash: ef2e10182fd1861e68b821491916327c25416ca7ae70e5a6e43313dbc71fe993
dependencies: dependencies:
@ -465,5 +464,5 @@ direct_dependencies:
- espressif/arduino-esp32 - espressif/arduino-esp32
- idf - idf
manifest_hash: bb586ec925aec51fa9a2f4da694bbf1dd0c5ef5f4f654f1ef56d33c48f29627b manifest_hash: bb586ec925aec51fa9a2f4da694bbf1dd0c5ef5f4f654f1ef56d33c48f29627b
target: esp32s3 target: esp32
version: 2.0.0 version: 2.0.0

6
main/CMakeLists.txt
View File

@ -1,7 +1,5 @@
file(GLOB_RECURSE SOURCES "*.cpp") idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp"
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen)
idf_component_register(SRCS "${SOURCES}"
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab drone_comms)
target_compile_options(${COMPONENT_LIB} PRIVATE target_compile_options(${COMPONENT_LIB} PRIVATE
"-Wno-gnu-array-member-paren-init" "-Wno-gnu-array-member-paren-init"

146
main/drone.cpp
View File

@ -1,146 +0,0 @@
#include "drone.h"
#include "DShotRMT.h"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "driver/rmt_tx.h"
#include "drone_comms.h"
#include "drone_controller.h"
#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"
#include "freertos/task.h"
#include "imu.h"
#include "nav.h"
#include "packet_handler.h"
#include "sens_fus.h"
#include "soc/gpio_num.h"
#include <algorithm>
#include <cstdint>
#include <cstring>
#include <optional>
#include <stdlib.h>
#define CONTROLLER_TASK_FREQUENCY 1000.0;
dcont::ControllerConfig default_config() {
dcont::ControllerConfig config;
// 1. Configure the PID Stack
// Note: kp, ki, kd are arrays of 3 [roll, pitch, yaw]
// Position Loop (Position -> Velocity)
config.stack.position_pid = {
.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/Rotation)
config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
.ki = {0.01f, 0.01f, 0.01f},
.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 = {.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 = 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
float mixer[4][3] = {
// x, y, z
//
{-1.0, -1.0, -1.0}, // Rear Right
{-1.0, 1.0, 1.0}, // Rear Left
{1.0, -1.0, 1.0}, // Front Right
{1.0, 1.0, -1.0}, // Front Left
};
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 3; j++) {
config.motor_map[i][j] = mixer[i][j];
}
}
return config;
}
constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
void drone_controller_task(void *params) {
drone_cont = new drone_cont_state;
drone_cont->init();
while (true) {
drone_cont->update();
vTaskDelay(pdMS_TO_TICKS(1));
}
}
const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
GPIO_NUM_15};
// const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_14, GPIO_NUM_46,
// GPIO_NUM_16};
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.02;
motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
motors[i]->begin();
}
// ARM
unsigned long armTime = millis();
while (millis() - armTime < 5000) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(0);
}
vTaskDelay(2);
}
unsigned long armTime_sec = millis();
while (millis() - armTime_sec < 1000) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(20 * (millis() - armTime_sec) /
armTime_sec);
}
vTaskDelay(2);
}
while (true) {
for (int i = 0; i < 4; i++) {
float throttle =
std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f;
if (atomic_load(&killswitch_active)) {
throttle = 0.0;
}
motors[i]->sendThrottlePercent(throttle);
}
vTaskDelay(2);
}
}

231
main/drone.h
View File

@ -1,231 +0,0 @@
#pragma once
#include "esp_log.h"
#include "nav.h"
#include "esp32-hal.h"
#include <atomic>
#include <cmath>
#include <cstdint>
#include <stdatomic.h>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "drone_comms.h"
#include "drone_controller.h"
#include "packet_handler.h"
#include "imu.h"
#include "packet_handler.h"
#include "sens_fus.h"
#define CONNECTION_LOST_THRESHOLD 200
#define MAX_LANDING_LINVEL 1.0
void setup_drone();
inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; }
void drone_controller_task(void *params);
void motor_throttles_task(void *params);
// 3 4 2 1
inline float *motor_throttles = nullptr;
inline std::atomic_bool killswitch_active = false;
dcont::ControllerConfig default_config();
struct drone_cont_state {
bool angvel_stablilized;
bool fall_vel_stabilized;
Eigen::Vector3f pos;
Eigen::Vector3f vel;
Eigen::Quaternionf rot;
Eigen::Vector3f angvel;
dcont::StackedController *drone_controller;
atomic_uint_fast8_t current_input_mode = INPUT_TYPE::ACRO;
void init() { this->drone_controller = dcont::create(default_config()); }
void drone_cont_stabilize() {
// 1. ANGLE VELOCITY STABILIZATION
// Kill the spin first to ensure sensors/control loops can work accurately.
if (!this->angvel_stablilized) {
dcont::set_input(drone_controller,
dcont::Input{.joystick = {.throttle_input = 0.0,
.roll_input = 0.0,
.yaw_input = 0.0,
.pitch_input = 0.0},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {0.0, 0.0, 0.0},
.velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Acro});
if (this->angvel.norm() < 1.0) {
this->angvel_stablilized = true;
}
}
// 2. FALL VELOCITY STABILIZATION (WITH PROP WASH AVOIDANCE)
// Instead of climbing straight up, we move at an angle.
if (!this->fall_vel_stabilized) {
dcont::set_input(drone_controller,
dcont::Input{.joystick = {.throttle_input = 1.0,
.roll_input = 0.0,
.yaw_input = 0.0,
.pitch_input = 0.0},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {0.0, 0.05 * M_PI, 0.0},
.velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Rotation});
if (this->vel.z() - 1.0 >= 0) {
this->fall_vel_stabilized = true;
}
}
dcont::set_input(drone_controller,
dcont::Input{.joystick = {.throttle_input = 0.0,
.roll_input = 0.0,
.yaw_input = 0.0,
.pitch_input = 0.0},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {0.0, 0.0, 0.0},
.velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Velocity});
}
inline bool stabilization_done() {
return this->fall_vel_stabilized && this->angvel_stablilized;
}
void fetch_sens() {
static imu_state imu_state_local;
if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) {
imu_state_local = imu_state_var;
xSemaphoreGive(imu_state_mutex);
this->angvel = imu_state_local.angvel;
}
if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
this->pos = sens_fus.position;
this->vel = sens_fus.velocity;
xSemaphoreGive(sens_fus_mutex);
}
this->rot = imu_state_var.rot;
}
void update_controller_sens() {
dcont::set_cur_time(drone_controller, millis() / 1000.0f);
dcont::set_cur_angvel(drone_controller, v3f_to_v3c(this->angvel));
dcont::set_cur_linvel(drone_controller, v3f_to_v3c(this->vel));
dcont::set_cur_pos(drone_controller, v3f_to_v3c(this->pos));
dcont::set_cur_rot(drone_controller, dcont::QuatC{.i = this->rot.x(),
.j = this->rot.y(),
.k = this->rot.z(),
.w = this->rot.w()});
}
void update_throttles() {
auto thrt_var = dcont::get_throttles(drone_controller);
auto thrt = thrt_var.values;
memcpy(motor_throttles, thrt, sizeof(float) * 4);
// ESP_LOGI("CONT", "thr: %f, %f, %f, %f", thrt[0], thrt[1], thrt[2],
// thrt[3]); ESP_LOGE("CONT", "UPDATE THROTTLES");
}
void update_input() {
packet_controller_input c;
switch (atomic_load(&this->current_input_mode)) {
case INPUT_TYPE::ACRO: {
if (xSemaphoreTake(controller_input_semaphore, 10)) {
// if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) {
// current_controller_input = {0, 0, 0, 0};
// }
c = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
dcont::set_input(
drone_controller,
dcont::Input{.joystick = {.throttle_input = c.ly,
.roll_input = c.rx,
.yaw_input = c.lx,
.pitch_input = c.ry},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {c.ry * 3, c.rx * 3, c.lx * 3},
.velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Rotation});
} else {
drone_cont_stabilize();
}
} break;
case INPUT_TYPE::AUTO_NAV: {
if (!stabilization_done()) {
drone_cont_stabilize();
} else if (xSemaphoreTake(nav_mutex, 10)) {
waypoint wayp = nav_man.get_current_waypoint();
if (nav_man.current_waypoint == 8) {
dcont::set_max_linvel(this->drone_controller, MAX_LANDING_LINVEL);
}
xSemaphoreGive(nav_mutex);
if (wayp.coords_in_axis == std::nullopt) {
drone_cont_stabilize();
} else {
auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero());
dcont::set_input(
drone_controller,
dcont::Input{.joystick = {.throttle_input = 0.0,
.roll_input = 0.0,
.yaw_input = 0.0,
.pitch_input = 0.0},
.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 = dcont::ModeInput::Position});
}
} else {
drone_cont_stabilize();
}
break;
}
default:
case INPUT_TYPE::STABILIZE_FALL: {
drone_cont_stabilize();
break;
}
}
}
void update() {
this->fetch_sens();
this->update_controller_sens();
this->update_input();
this->update_throttles();
}
};
inline drone_cont_state *drone_cont = nullptr;

102
main/env_sens.cpp
View File

@ -1,48 +1,30 @@
#include "env_sens.h"
#include "nav.h"
#include "sens_fus.h"
#include "esp_log.h" #include "esp_log.h"
#include <Adafruit_BME280.h> #include <Adafruit_BME280.h>
#include <SPI.h> #include <SPI.h>
#include <Wire.h> #include <Wire.h>
#include <cmath>
#include "freertos/idf_additions.h" #define SEALEVELPRESSURE_HPA (1013.25)
#define SEALEVELPRESSURE_HPA (1030)
Adafruit_BME280 bme; // use I2C interface Adafruit_BME280 bme; // use I2C interface
Adafruit_Sensor *bme_temp = bme.getTemperatureSensor(); Adafruit_Sensor *bme_temp = bme.getTemperatureSensor();
Adafruit_Sensor *bme_pressure = bme.getPressureSensor(); Adafruit_Sensor *bme_pressure = bme.getPressureSensor();
Adafruit_Sensor *bme_humidity = bme.getHumiditySensor(); Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
static const constexpr char *TAG = "BARO"; static const constexpr char *TAG = "IMU";
#define BARO_SDA GPIO_NUM_47 // SDI
#define BARO_SCL GPIO_NUM_48
namespace env_sens { namespace env_sens {
void setup() { void setup() {
baro_mutex = xSemaphoreCreateMutex(); if (!bme.begin()) {
TwoWire *wire = new TwoWire(0);
wire->begin(BARO_SDA, BARO_SCL);
if (!bme.begin(119, wire)) {
ESP_LOGE(TAG, "Couldn't find a valid sensor"); ESP_LOGE(TAG, "Couldn't find a valid sensor");
ESP.restart();
return; return;
} }
ESP_LOGI(TAG, "BARO SETUP COMPLETE."); bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X2,
bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_X2, Adafruit_BME280::SAMPLING_NONE,
Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_NONE, Adafruit_BME280::FILTER_OFF,
Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_500); Adafruit_BME280::STANDBY_MS_62_5);
bme_temp->printSensorDetails(); bme_temp->printSensorDetails();
bme_pressure->printSensorDetails(); bme_pressure->printSensorDetails();
@ -50,41 +32,21 @@ void setup() {
} }
float get_temperature() { float get_temperature() {
if (millis() - env_sens::time_term_read > 20) {
sensors_event_t temp_event; sensors_event_t temp_event;
if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
bme_temp->getEvent(&temp_event); bme_temp->getEvent(&temp_event);
xSemaphoreGive(baro_mutex);
}
return temp_event.temperature; return temp_event.temperature;
} }
return env_sens::term_read;
}
float get_pressure() { float get_pressure() {
if (millis() - env_sens::time_baro_read > 20) {
sensors_event_t e; sensors_event_t e;
if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
bme_pressure->getEvent(&e); bme_pressure->getEvent(&e);
xSemaphoreGive(baro_mutex);
}
return e.pressure; return e.pressure;
} }
return env_sens::baro_read;
}
float calculateAltitude(float pressure, float seaLevelPressure, float calculateAltitude(float pressure, float seaLevelPressure,
float tempCelsius) { float tempCelsius) {
float altitude = float altitude =
(((std::pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) * (((pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
(tempCelsius + 273.15)) / (tempCelsius + 273.15)) /
0.0065; 0.0065;
return altitude; return altitude;
@ -100,50 +62,4 @@ void dbg_sens() {
get_pressure(), get_altitude()); get_pressure(), get_altitude());
} }
void baro_poll_task(void *_) {
env_sens::setup();
float last_alt = env_sens::get_altitude();
uint32_t last_time = xTaskGetTickCount();
float filtered_alt = last_alt;
const float alt_lpf = 0.1f;
while (true) {
uint32_t now = xTaskGetTickCount();
float dt = (now - last_time) * portTICK_PERIOD_MS / 1000.0f;
if (dt > 0.001f) { // Prevent division by zero
float current_alt = env_sens::get_altitude();
if (current_alt == INFINITY) {
continue;
}
filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt;
float v_z = (filtered_alt - last_alt) / dt;
Eigen::Vector3f baro_pos = sens_fus.position;
baro_pos.z() = filtered_alt;
Eigen::Vector3f baro_vel = sens_fus.velocity;
baro_vel.z() = v_z;
if (sens_fus_mutex &&
xSemaphoreTake(sens_fus_mutex, (TickType_t)20) == pdTRUE) {
// Update the filter with Baro data
sens_fus.measure_baro(dt, baro_pos, baro_vel);
xSemaphoreGive(sens_fus_mutex);
}
last_alt = filtered_alt;
last_time = now;
}
// BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
vTaskDelay(pdMS_TO_TICKS(500));
}
}
} // namespace env_sens } // namespace env_sens

21
main/env_sens.h
View File

@ -1,17 +1,5 @@
#pragma once #pragma once
#include <cstdint>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "freertos/idf_additions.h"
namespace env_sens { namespace env_sens {
void setup(); void setup();
@ -23,14 +11,5 @@ float get_pressure();
void dbg_sens(); void dbg_sens();
float get_altitude(); 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 } // namespace env_sens
inline SemaphoreHandle_t baro_mutex = NULL;

64
main/gps.cpp
View File

@ -1,64 +0,0 @@
#include "gps.h"
#include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "sens_fus.h"
#include <cstdint>
static const char *TAG = "GPS_TASK";
void gps_poll_task(void *_) {
gps = new GPS();
gps->begin();
gps_mutex = xSemaphoreCreateMutex();
ESP_LOGI(TAG, "GPS TASK INIT.");
uint64_t last_time = millis();
while (true) {
bool available = false;
Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
std::optional<Eigen::Vector3f> local_coords;
if (xSemaphoreTake(gps_mutex, pdMS_TO_TICKS(50)) == pdTRUE) {
gps->poll();
if (gps->gps_avaliable()) {
local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
local_coords = gps->get_coordinates();
available = local_coords.has_value();
}
xSemaphoreGive(gps_mutex);
} else {
ESP_LOGE(TAG, "FAILED TO GET GPS 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));
xSemaphoreGive(sens_fus_mutex);
last_time = current_time;
} else {
ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
}
} else {
if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
sens_fus.gps_lost();
xSemaphoreGive(sens_fus_mutex);
}
}
}
vTaskDelay(pdMS_TO_TICKS(5)); // 10Hz polling
}
}

136
main/gps.h
View File

@ -1,136 +0,0 @@
#pragma once
#include <cstdint>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "HardwareSerial.h"
#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_TX_PIN 18
#define GPS_RX_PIN 17
struct GPS {
Adafruit_GPS *gps;
float origin_lat;
float origin_long;
GPS() {
origin_lat = INFINITY;
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_TX_PIN, GPS_RX_PIN);
this->gps = new Adafruit_GPS(&Serial2);
// this->gps->begin(9600);
this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
}
bool gps_avaliable() { return this->gps->fix; }
std::optional<Eigen::Vector2f> velocity() {
if (!this->gps->fix) {
return std::nullopt;
}
float speed = this->gps->speed * KNOTS_TO_M_SEC;
float angle = this->gps->angle * TO_RAD;
return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed;
}
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;
float meanLat = (this->origin_lat) * TO_RAD;
float y = dLat * earth_radius;
float x = dLon * earth_radius * std::cos(meanLat);
float z = this->gps->altitude;
return Eigen::Vector3f(x, y, z);
}
std::optional<Eigen::Vector3f> get_coordinates() {
if (this->gps->fix == false) {
return std::nullopt;
}
float latitude = this->gps->latitudeDegrees;
float longitude = this->gps->longitudeDegrees;
return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude);
}
void poll() {
for (int i = 0; i < 50; i++) {
char _ = this->gps->read();
}
if (this->gps->newNMEAreceived()) {
char *line = this->gps->lastNMEA();
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
this->gps->parse(line);
}
}
};
inline SemaphoreHandle_t gps_mutex;
inline GPS *gps = nullptr;
void gps_poll_task(void *_);

150
main/imu.cpp
View File

@ -1,152 +1,68 @@
#include "imu.h" #include "imu.h"
#include "Eigen/Geometry"
#include "Esp.h"
#include "drone_controller.h"
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
#include "esp_log.h" #include "esp_log.h"
#include "esp_timer.h" #include "esp_timer.h"
#include "hal/spi_types.h"
#include "sens_fus.h"
#include "freertos/idf_additions.h" #include "freertos/idf_additions.h"
static const char *TAG = "IMU"; static const char *TAG = "IMU";
#define IMU_CS GPIO_NUM_3 // Green SemaphoreHandle_t imuStateMutex = NULL;
#define IMU_MOSI GPIO_NUM_2 // DI - White
#define IMU_RST GPIO_NUM_1 // Red
#define IMU_INT GPIO_NUM_4 // Yellow BNO08x *setup_imu(imu_state *state) {
#define IMU_MISO GPIO_NUM_5 // SDA - White BNO08x *imu = new BNO08x();
#define IMU_SCLK GPIO_NUM_6 // SCL - Green
BNO08x *setup_imu() {
imu_state *local_state = new imu_state;
imu_state_mutex = xSemaphoreCreateMutex();
BNO08x *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()) { if (!imu->initialize()) {
ESP_LOGE(TAG, "BNO08x Init failure."); ESP_LOGE(TAG, "BNO08x Init failure.");
ESP.restart(); return nullptr;
}
if (imuStateMutex == NULL) {
imuStateMutex = xSemaphoreCreateMutex();
} }
imu->print_product_ids();
imu->dynamic_calibration_autosave_enable(); imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all); imu->dynamic_calibration_enable(BNO08xCalSel::all);
// imu->rpt.rv_game.enable(2500UL); imu->rpt.rv_game.enable(2500UL);
imu->rpt.rv.enable(2500UL);
// imu->rpt.cal_magnetometer.enable(10000UL);
imu->rpt.linear_accelerometer.enable(2500UL); imu->rpt.linear_accelerometer.enable(2500UL);
// imu->rpt.accelerometer.enable(10000UL); // imu->rpt.rv_game.tare();
imu->rpt.cal_gyro.enable(2500UL);
imu->register_cb([imu, local_state]() { imu->register_cb([imu, state]() {
// ESP_LOGI("IMU", "CALLBACK"); if (imu->rpt.rv_game.has_new_data()) {
bool needs_updating = false; auto sens_rot = imu->rpt.rv_game.get_quat();
if (sens_fus_mutex == NULL || imu_state_mutex == NULL) auto euler = imu->rpt.rv_game.get_euler();
return; if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
state->euler_ang = {euler.x, euler.y, euler.z};
if (imu->rpt.rv.has_new_data()) { state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
xSemaphoreGive(imuStateMutex);
needs_updating = true;
auto sens_rot = imu->rpt.rv.get_quat();
auto sens_euler = imu->rpt.rv.get_euler();
local_state->rot =
Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k);
// ESP_LOGI("ROT", "(%f, %f, %f, %f)", sens_rot.real, sens_rot.i,
// sens_rot.j,
// sens_rot.k);
// Eigen::Quaternionf q_global_yaw(
// Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
// local_state->rot = q_global_yaw * local_state->rot;
// local_state->rot.normalize();
// {.i = sens_rot.i, .j = sens_rot.j, .k = sens_rot.k, .w =
// sens_rot.real}; 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()) {
needs_updating = true;
auto cal_gyro = imu->rpt.cal_gyro.get();
local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z};
} }
if (imu->rpt.linear_accelerometer.has_new_data()) { if (imu->rpt.linear_accelerometer.has_new_data()) {
needs_updating = true; if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
auto sens_accel = imu->rpt.linear_accelerometer.get();
auto sens_accel_lin = imu->rpt.linear_accelerometer.get(); state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
local_state->lin_accel = {sens_accel_lin.x, sens_accel_lin.y,
sens_accel_lin.z};
// ESP_LOGI("IMU-ac", "lin_accel %f,%f,%f", sens_accel_lin.x,
// sens_accel_lin.y, sens_accel_lin.z);
int64_t current_time = esp_timer_get_time(); int64_t current_time = esp_timer_get_time();
if (local_state->last_time != -1) { if (state->last_time != -1) {
float dt = (current_time - state->last_time) / 1000000.0f;
float dt = Vec3C delta = apply_rot(&state->accel, &state->rot);
((float)(current_time - local_state->last_time)) / 1000000.0f;
dcont::QuatC quatc{.i = local_state->rot.x(), // Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt
.j = local_state->rot.y(), delta.x += state->last_accel.x;
.k = local_state->rot.z(), delta.y += state->last_accel.y;
.w = local_state->rot.w()}; delta.z += state->last_accel.z;
dcont::Vec3C accel_global = multiply_vec_by(&delta, dt * 0.5f);
dcont::apply_rot(&local_state->lin_accel, &quatc); add_to_vec(&state->vel, &delta);
local_state->lin_accel_global = accel_global; state->last_accel = state->accel;
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);
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.");
}
}
local_state->last_time = current_time;
} }
state->last_time = current_time;
if (needs_updating) { xSemaphoreGive(imuStateMutex);
if (xSemaphoreTake(imu_state_mutex, 0)) {
imu_state_var = *local_state;
xSemaphoreGive(imu_state_mutex);
} else {
ESP_LOGE(TAG, "FAILED TO GET IMU STATE MUTEX");
} }
} }
}); });

40
main/imu.h
View File

@ -1,40 +1,20 @@
#pragma once #pragma once
#include "BNO08x.hpp"
#include "BNO08xGlobalTypes.hpp"
#include "drone_controller.h" #include "drone_controller.h"
#include <cmath>
#include "freertos/idf_additions.h"
#include <cstdint> #include <cstdint>
#ifdef LOW
#undef LOW
#endif
#ifdef HIGH
#undef HIGH
#endif
#include "BNO08x.hpp"
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
struct imu_state { struct imu_state {
dcont::Vec3C lin_accel = {0, 0, 0}; Vec3C accel = {0, 0, 0};
dcont::Vec3C lin_accel_global = {0, 0, 0}; Vec3C last_accel = {0, 0, 0};
// dcont::Vec3C accel = {0, 0, 0}; QuatC rot = {0, 0, 0, 1};
Eigen::Quaternionf rot = {0, 0, 0, 1}; Vec3C vel = {0, 0, 0};
int64_t last_time = -1; int64_t last_time = -1;
Eigen::Vector3f angvel; Vec3C euler_ang = {0, 0, 0};
Eigen::Vector3f rot_euler;
}; };
BNO08x *setup_imu(); BNO08x *setup_imu(imu_state *state);
inline SemaphoreHandle_t imu_state_mutex = NULL; extern SemaphoreHandle_t imuStateMutex;
inline imu_state imu_state_var;

236
main/main.cpp
View File

@ -1,228 +1,36 @@
#include "Eigen/Core" #include "esp_log.h"
#include "driver/gpio.h" #include "esp_timer.h"
#include "drone.h" #include <cmath>
#include "drone_comms.h" #include <cstdint>
#include "drone_controller.h" #include <stdio.h>
#include "esp32-hal.h" #include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include <atomic>
#include <cmath> #include <cstdint>
#include <optional>
#include "env_sens.h" #include "env_sens.h"
#include "gps.h" #include "freertos/idf_additions.h"
#include "imu.h" #include "imu.h"
#include "nav.h"
#include "packet_handler.h"
#include "radio.h"
#include "sens_fus.h"
#include "servo.h"
static const char *TAG = "MAIN"; static const constexpr char *TAG = "Main";
#define TIME_RELEASE_QUEUE_TO_ACTIVATION 500
extern "C" void app_main(void) { extern "C" void app_main(void) {
env_sens::setup();
sens_fus_mutex = xSemaphoreCreateMutex(); imu_state imu_state;
configASSERT(sens_fus_mutex); BNO08x *imu = setup_imu(&imu_state);
nav_mutex = xSemaphoreCreateMutex(); if (imu == nullptr) {
ESP_LOGE(TAG, "IMU setup failed.");
initArduino(); return;
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(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
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
NULL, // Parameters
24, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
);
// vTaskDelay(5000);
// for (int i = 0; i < 4; i++) {
// motor_throttles[i] = 10.0;
// vTaskDelay(2000);
// motor_throttles[i] = 0.0;
// }
xTaskCreate(
[](void *pvParameters) {
while (true) {
while (packet_rx_queue &&
xQueueReceive(packet_rx_queue, &packet_data[0], 20)) {
handle_packet(&packet_data[0]);
}
vTaskDelay(1);
}
},
"lambda_recv_task", 8192, NULL, 5, NULL);
setup_imu();
servo_init();
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_position_broadcast_time = 0;
uint64_t last_status_broadcast_time = 0;
uint64_t time_activation_queue = 0;
bool released = false;
while (true) {
if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
last_position_broadcast_time = millis();
}
// auto vel = sens_fus.velocity;
// ESP_LOGI("GPSvsIMU", "gps_vel: %f, imu_vel: %f",
// gps->velocity().value().norm(),
// Eigen::Vector3f(vel.x(), vel.y(), 0.0).norm());
if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS);
last_status_broadcast_time = millis();
} }
if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) { ESP_LOGE(TAG, "IMU setup sucess.");
if (gps->gps_avaliable()) { while (1) {
waypoint current_wayp = nav_man.get_current_waypoint(); vTaskDelay(pdMS_TO_TICKS(300));
auto pos = sens_fus.position; if (xSemaphoreTake(imuStateMutex, pdMS_TO_TICKS(5)) == pdTRUE) {
ESP_LOGI(TAG, "accel - %f, %f, %f", imu_state.accel.x, imu_state.accel.y,
if ((current_wayp.coords_in_axis.value_or( imu_state.accel.z);
Eigen::Vector3f(INFINITY, INFINITY, INFINITY)) - ESP_LOGI(TAG, "; euler - %f, %f, %f\n", imu_state.euler_ang.x,
pos) imu_state.euler_ang.y, imu_state.euler_ang.z);
.norm() < 1.0) { xSemaphoreGive(imuStateMutex);
nav_man.waypoint_reached();
}
}
xSemaphoreGive(nav_mutex);
}
// Release
if (imu_state_var.lin_accel_global.z < -8.0 && !released) {
released = true;
time_activation_queue = millis();
}
if (released && drone_cont &&
std::atomic_load(&drone_cont->current_input_mode) ==
INPUT_TYPE::AUTO_NAV &&
millis() - time_activation_queue > TIME_RELEASE_QUEUE_TO_ACTIVATION) {
dcont::reset_pid_states(drone_cont->drone_controller);
servo_set(SERVO_OPTIONS::UP);
xTaskCreate(
[](void *pvParameters) {
vTaskDelay(pdMS_TO_TICKS(700));
std::atomic_store(&killswitch_active, false);
},
"lambda_task", 2048, NULL, 5, NULL);
}
// Logging
if (millis() > last_print_time + 1000) {
last_print_time = millis();
std::optional<Eigen::Vector3f> coords;
float lat, lon, alt;
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, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
D_pos[2]);
}
} }
env_sens::dbg_sens(); env_sens::dbg_sens();
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(sens_fus_mutex);
}
if (nav_data_ready) {
ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1],
local_pos[2]);
ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
local_vel[2]);
}
if (motor_throttles != nullptr) {
ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0],
motor_throttles[1], motor_throttles[2], motor_throttles[3]);
}
if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) {
ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)",
current_controller_input.lx, current_controller_input.ly,
current_controller_input.rx, current_controller_input.ry);
}
// ESP_LOGI(TAG, "ROT: (%f, %f, %f)", imu_state_var.rot_euler.x(),
// imu_state_var.rot_euler.y(), imu_state_var.rot_euler.z());
}
vTaskDelay(pdMS_TO_TICKS(5));
} }
} }

74
main/nav.h
View File

@ -1,74 +0,0 @@
#pragma once
#include "drone_controller.h"
#include "freertos/idf_additions.h"
#include "gps.h"
#include <cmath>
#include <cstdint>
#include <optional>
#define WAYPOINT_COUNT 8
struct waypoint {
Eigen::Vector3f coords; // lat, lon, alt
std::optional<Eigen::Vector3f> coords_in_axis = std::nullopt;
bool active = false; // active or to be skipped
bool landing = false;
};
struct drone_nav {
waypoint waypoints[WAYPOINT_COUNT + 1];
uint8_t current_waypoint;
void set_active_mask(uint8_t mask) {
for (int i = 0; i < WAYPOINT_COUNT; i++) {
this->waypoints[i].active = (mask & (1 << i)) != 0;
}
}
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;
}
waypoint get_current_waypoint() {
waypoint wayp = this->waypoints[this->current_waypoint];
if (!wayp.coords_in_axis.has_value()) {
auto axis =
gps->waypoint_to_xyz(wayp.coords[0], wayp.coords[1], wayp.coords[2]);
wayp.coords_in_axis = axis;
}
return wayp;
}
void waypoint_reached() {
waypoint wayp = waypoints[this->current_waypoint];
if (wayp.landing) {
this->waypoints[8] = waypoint{
.coords = Eigen::Vector3f(wayp.coords.x(), wayp.coords.y(), 0.0f),
.coords_in_axis = std::nullopt};
current_waypoint = 8;
return;
}
int i = this->current_waypoint + 1;
while (i != this->current_waypoint) {
bool active = waypoints[i].active;
if (active) {
this->current_waypoint = i;
}
i++;
i = i % 8;
}
}
};
inline SemaphoreHandle_t nav_mutex = NULL;
inline drone_nav nav_man;

177
main/packet_handler.cpp
View File

@ -1,177 +0,0 @@
#include "packet_handler.h"
#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 <atomic>
#include <cstdint>
#include <sys/unistd.h>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
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) {
packet_command_request *packet =
(packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
send_packet_getter(packet->packet_requested);
}
// NAV SETTERS
else 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);
} else if (packet_type == PACKET_TYPE::DRONE_NAV) {
handle_nav_update(packet_addr);
} else if (packet_type == PACKET_TYPE::KILLSWITCH_TOGGLE) {
packet_killswitch_toggle *packet =
(packet_killswitch_toggle *)(packet_addr + sizeof(PACKET_TYPE));
std::atomic_store(&killswitch_active, packet->killswitch_active);
ESP_LOGI("RADIO_RX", "KILLSWITCH PAACKET");
ESP_LOGE("switch_set", "killswitch set to: %d, packet: %d",
atomic_load(&killswitch_active), packet->killswitch_active);
} else if (packet_type == PACKET_TYPE::MODE_INPUT) {
packet_mode_input *packet =
(packet_mode_input *)(packet_addr + sizeof(PACKET_TYPE));
atomic_store(&drone_cont->current_input_mode, packet->input_type);
} else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) {
packet_controller_input *packet =
(packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
if (xSemaphoreTake(controller_input_semaphore, 10)) {
current_controller_input = *packet;
time_last_controller = millis();
xSemaphoreGive(controller_input_semaphore);
}
}
}
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 send_packet_getter(PACKET_TYPE requested_type) {
std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
imu_state state;
if (xSemaphoreTake(imu_state_mutex, 20)) {
state = imu_state_var;
xSemaphoreGive(imu_state_mutex);
}
Eigen::Vector3f local_vel = state.rot.inverse() * sens_fus.velocity;
ESP_LOGE("Radio tx", "[f,%f,%f,%f]", state.rot.w(), state.rot.x(),
state.rot.y(), state.rot.z());
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
.trans = {sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()},
.vel = {local_vel.x(), local_vel.y(), local_vel.z()},
.rot = {state.rot.w(), state.rot.x(), state.rot.y(), state.rot.z()},
});
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
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 = gps->unix_timestamp_millis(),
.gps_fix = gps->gps_avaliable()});
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::KILLSWITCH_TOGGLE) {
bool value = atomic_load(&killswitch_active);
resp_packet = create_packet_pooled(PACKET_TYPE::KILLSWITCH_TOGGLE,
packet_killswitch_toggle{value});
}
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;
bool land = false;
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
coords = nav_man.waypoints[index].coords;
land = nav_man.waypoints[index].landing;
xSemaphoreGive(nav_mutex);
}
resp_packet = create_packet_pooled(
requested_type,
packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}, land});
}
if (resp_packet.first != nullptr) {
xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
}
}

16
main/packet_handler.h
View File

@ -1,16 +0,0 @@
#pragma once
#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;

145
main/radio.cpp
View File

@ -1,145 +0,0 @@
#include "radio.h"
#include "Esp.h"
#include "esp32-hal-gpio.h"
#include "esp32-hal-spi.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "packet_handler.h"
#include <RFM69.h>
#include <SPI.h>
#include <cstdint>
#include <cstring>
#include <drone_comms.h>
// Right to left on hardware.
#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
#define NETWORKID 100
static const char *TAG = "RADIO_TASK";
#define PACKET_QUEUE_CAP 10
static bool is_pending_switch = false;
static uint32_t target_bitrate = 0;
static uint32_t switch_at_ms = 0;
static uint32_t rollback_at_ms = 0;
static bool confirmed_at_new_rate = false;
void radio_task(void *pvParameters) {
ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID());
packet_rx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
packet_tx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
controller_input_semaphore = xSemaphoreCreateMutex();
pinMode(RFM69_CS, OUTPUT);
pinMode(RFM69_INT, INPUT);
SPIClass hspi(HSPI);
hspi.begin(RFM69_SCLK, RFM69_MISO, RFM69_MOSI, RFM69_CS);
pinMode(RFM69_RST, OUTPUT);
digitalWrite(RFM69_RST, HIGH);
vTaskDelay(pdMS_TO_TICKS(10));
digitalWrite(RFM69_RST, LOW);
vTaskDelay(pdMS_TO_TICKS(50));
RFM69 radio(RFM69_CS, RFM69_INT, true, &hspi);
if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
radio.setPowerLevel(31);
radio.setHighPower(true);
// radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
radio.readAllRegsCompact();
} else {
ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
ESP.restart();
}
while (1) {
uint32_t now = millis();
if (radio.receiveDone()) {
// If we receive ANY valid packet while in probation, confirm the switch
// (Bit-rate switching)
if (is_pending_switch && now > switch_at_ms) {
confirmed_at_new_rate = true;
ESP_LOGI(TAG, "New bitrate confirmed by valid packet.");
}
ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID, radio.RSSI,
radio.DATALEN);
memset(packet_data, '\0', sizeof(packet_data));
memcpy(packet_data, radio.DATA, radio.DATALEN);
PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
if (packet_type == COMMAND_CHANGE_DATARATE) {
packet_command_datarate *cmd =
(packet_command_datarate *)(&packet_data[0] + sizeof(PACKET_TYPE));
target_bitrate = cmd->target_bitrate;
switch_at_ms = now + cmd->ms_delay;
rollback_at_ms = now + cmd->ms_rollback;
is_pending_switch = true;
confirmed_at_new_rate = false;
ESP_LOGI(TAG, "Datarate change requested: %d. Switching in 100ms...",
target_bitrate);
} else {
ESP_LOGI(TAG, "RECVD PACKET");
xQueueSend(packet_rx_queue, &packet_data[0], portMAX_DELAY);
}
if (radio.ACKRequested()) {
radio.sendACK();
}
}
// Send packets that were queued up for sending
if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
radio.send(GROUNDID, &packet_data[0], get_packet_size(packet_type));
}
// --- STATE MACHINE FOR BITRATE SWITCHING ---
// 1. Execute the Switch
if (is_pending_switch && now >= switch_at_ms && !confirmed_at_new_rate) {
// We only want to trigger the register write once
radio.setCustomBitrate(target_bitrate);
switch_at_ms = 0xFFFFFFFF; // Prevent re-triggering
}
// 2. The Rollback (The Fail-safe)
if (is_pending_switch && !confirmed_at_new_rate && now > rollback_at_ms) {
ESP_LOGE(TAG,
"ROLLBACK: No confirmation at new rate. Reverting to default.");
radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
is_pending_switch = false;
}
// 3. Clear pending flag once confirmed
if (confirmed_at_new_rate) {
is_pending_switch = false;
confirmed_at_new_rate = false;
}
vTaskDelay(pdMS_TO_TICKS(1));
}
}

9
main/radio.h
View File

@ -1,9 +0,0 @@
#pragma once
#include "drone_comms.h"
#include "freertos/idf_additions.h"
inline QueueHandle_t packet_rx_queue = NULL;
inline QueueHandle_t packet_tx_queue = NULL;
void radio_task(void *pvParameters);

110
main/sens_fus.h
View File

@ -1,110 +0,0 @@
#pragma once
#include "Eigen/Core"
#include <cmath>
#include <cstdlib>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#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());
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();
Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
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 = {0.1f, 0.1f, 0.0f};
Eigen::Vector3f position_error_mult = {0.1f, 0.1f, 0.0f};
/*
* Tau is the time that the filter takes to reach 1-e^(-1) of the difference
* to a constant target
* 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.0};
Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, INFINITY};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f};
Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
void predict(float dt, Eigen::Vector3f accel) {
Eigen::Vector3f accel_err_rmvd =
accel.array() -
(this->velocity_error.array() * this->velocity_error_mult.array()) / dt;
Eigen::Vector3f next_velocity =
this->velocity + (this->last_accel_world + accel_err_rmvd) * 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)) *
dt;
this->velocity = next_velocity;
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);
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();
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) {
// 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;

52
main/servo.cpp
View File

@ -1,52 +0,0 @@
#include "servo.h"
#include "driver/ledc.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#define SERVO_PIN 7
#define LEDC_TIMER LEDC_TIMER_0
#define LEDC_MODE LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL LEDC_CHANNEL_0
#define LEDC_DUTY_RES LEDC_TIMER_13_BIT
#define LEDC_FREQUENCY 50
uint32_t us_to_duty(int us) {
// 20000 is the period in microseconds (50Hz)
// 8191 is the max duty cycle for 13-bit resolution
return (uint32_t)((float)us / 20000.0f * 8191.0f);
}
ledc_timer_config_t ledc_timer = {.speed_mode = LEDC_MODE,
.duty_resolution = LEDC_DUTY_RES,
.timer_num = LEDC_TIMER,
.freq_hz = LEDC_FREQUENCY,
.clk_cfg = LEDC_AUTO_CLK};
ledc_channel_config_t ledc_channel = {.gpio_num = SERVO_PIN,
.speed_mode = LEDC_MODE,
.channel = LEDC_CHANNEL,
.timer_sel = LEDC_TIMER,
.duty = 0};
void servo_init() {
ledc_timer_config(&ledc_timer);
ledc_channel_config(&ledc_channel);
}
#define DUTY_CYCLE_DOWN 2000
#define DUTY_CYCLE_UP 850
void servo_set(SERVO_OPTIONS opt) {
switch (opt) {
case UP:
ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_UP));
ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
break;
case DOWN:
ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_DOWN));
ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
case OFF:
ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, 0);
ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
}
}

11
main/servo.h
View File

@ -1,11 +0,0 @@
#pragma once
enum SERVO_OPTIONS {
UP,
DOWN,
OFF,
};
void servo_init();
void servo_set(SERVO_OPTIONS opt);

2
sdkconfig.defaults
View File

@ -1,2 +0,0 @@
CONFIG_FREERTOS_HZ=1000