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

last minute desperation

This commit is contained in:
franchioping 2026-04-24 01:41:23 +01:00
parent adb7c41f77
commit d52177e56a
14 changed files with 1104 additions and 2744 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_comms

@ -1 +1 @@
Subproject commit 7c49ec36fffcd7e0101e435aa5d94640f30dc3eb
Subproject commit ee39a37b9187a3a95570980af425115841f45394

2
logs/analyze.py
View File

@ -6,7 +6,7 @@ import pandas as pd
from pandas.io.parsers.readers import csv
# 1. Define your log file path
file_path = "output_9.log"
file_path = "output_10.log"
# 2. Extract and parse data
cleaned_data = []

3196
logs/out.csv
View File

File diff suppressed because it is too large Load Diff

79
main/drone.cpp
View File

@ -6,19 +6,19 @@
#include "driver/rmt_tx.h" #include "drone_comms.h"
#include "drone_controller.h"
#include "esp_timer.h" #include "imu.h" #include "logger.h"
#include "nav.h"
#include "packet_handler.h"
#include "sens_fus.h"
#include "dshot_definitions.h"
#include "esp32-hal.h"
#include "esp_log.h"
#include "esp_log_timestamp.h"
#include "esp_timer.h" #include "imu.h" #include "logger.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include "logger.h"
#include "nav.h"
#include "packet_handler.h"
#include "sens_fus.h"
#include "soc/gpio_num.h"
#include <algorithm>
#include <cstdint>
@ -36,42 +36,46 @@ dcont::ControllerConfig default_config() {
// Position Loop (Position -> Velocity)
config.stack.position_pid = {
.kp = {1.0f, 1.0f, 1.0f}, // kp
.kp = {0.1f, 0.1f, 0.1f}, // kp
.ki = {0.0f, 0.0f, 0.0f}, // ki
.kd = {0.0f, 0.0f, 0.0f}, // kd
.frequency = 25.0f // frequency (Hz)
.frequency = 5.0f // frequency (Hz)
};
// Velocity Loop (Velocity -> Acceleration/Rotation)
config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 8.0f},
.ki = {0.00f, 0.00f, 0.00f},
config.stack.linvel_pid = {.kp = {0.2f, 0.2f, 0.2f},
.ki = {0.01f, 0.01f, 0.01f},
// .ki = {0.01f, 0.01f, 0.01f},
.kd = {0.0f, 0.0f, 0.0f},
.frequency = 50.0f};
.integral_cap = {0.1f, 0.1f, 2.0f},
.frequency = 10.0f};
// Rotation Loop (Rotation/Accel -> Angular Rate)
config.stack.rotation_pid = {
.kp = {0.05f, 0.05f, 1.0f},
.ki = {0.0f, 0.0f, 0.2f},
.kp = {8.0f, 8.0f, 4.0f},
.ki = {0.2f, 0.2f, 0.2f},
.kd = {0.0f, 0.0f, 0.0f},
.integral_cap = {10.0f, 1.0f, 2.0f},
.frequency = 200.0f,
.integral_cap = {2.0f, 2.0f, 2.0f},
.frequency = 100.0f,
};
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
config.stack.rate_pid = {
.kp = {0.05f, 0.05f, 2.0f},
.kp = {0.02f, 0.02f, 0.2},
// .kp = {0.11f, 0.07f, 0.375},
// .kp = {0.05f, 0.05f, 2.0f},
.ki = {0.00f, 0.00f, 0.0f},
.kd = {0.00f, 0.00f, 0.0f},
.integral_cap = {1.0f, 1.0f, 1.0f},
.frequency = 400.0f,
};
// 2. Set Constraints
config.stack.max_rate = 3.14f; // ~180 degrees/s
config.stack.max_linvel = 3.0f; // 3 m/s
// 3. Physical Drone Properties
config.mass = 0.350f; // kg
config.max_thrust = 1.0f; // Newtons
config.stack.max_rate = 3.14f; // rad/s
config.stack.max_linvel = 4.0f;
config.stack.max_accel = 3.0;
config.mass = 0.325f; // kg
config.max_thrust = 1.8f; // Newtons
config.max_torque = 0.5f; // Nm
float mixer[4][3] = {
@ -92,23 +96,14 @@ dcont::ControllerConfig default_config() {
return config;
}
constexpr uint64_t wait_micro_sec = 1000000.0 / CONTROLLER_TASK_FREQUENCY;
const gpio_num_t motor_pins[4] = {GPIO_NUM_14, GPIO_NUM_16, GPIO_NUM_46,
GPIO_NUM_15};
DShotRMT *motors[4];
void drone_controller_task(void *params) {
drone_cont = new drone_cont_state;
drone_cont->init();
while (true) {
drone_cont->update();
vTaskDelay(1);
}
}
const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
GPIO_NUM_15};
DShotRMT *motors[4];
void motor_throttles_task(void *params) {
motor_throttles = (float *)malloc(sizeof(float) * 4);
for (int i = 0; i < 4; i++) {
@ -116,31 +111,27 @@ void motor_throttles_task(void *params) {
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);
motor_throttles[i] = 0.0;
}
vTaskDelay(2);
}
if (drone_cont != nullptr && drone_cont->drone_controller != nullptr) {
dcont::reset_pid_states(drone_cont->drone_controller);
}
while (true) {
drone_cont->update();
for (int i = 0; i < 4; i++) {
float throttle =
std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.6f;
float throttle = std::clamp(motor_throttles[i], 0.0f, 0.6f) * 100.0f;
if (atomic_load(&killswitch_active)) {
throttle = 0.0;
dcont::reset_pid_states(drone_cont->drone_controller);
}
motors[i]->sendThrottlePercent(throttle);
motors[i]->sendThrottlePercent(std::clamp(throttle, 0.0f, 60.0f));
}
vTaskDelay(2);
vTaskDelay(1);
}
}

204
main/drone.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "Eigen/Core"
#include "esp_log.h"
#include "gps.h"
#include "nav.h"
@ -29,7 +30,7 @@
#include "sens_fus.h"
#define CONNECTION_LOST_THRESHOLD 200
#define MAX_LANDING_LINVEL 1.0
#define MAX_LANDING_LINVEL 2.0
void setup_drone();
@ -41,7 +42,7 @@ void motor_throttles_task(void *params);
// 3 4 2 1
inline float *motor_throttles = nullptr;
inline std::atomic_bool killswitch_active = false;
inline std::atomic_bool killswitch_active = true;
dcont::ControllerConfig default_config();
@ -56,14 +57,19 @@ struct drone_cont_state {
Eigen::Vector3f angvel;
dcont::StackedController *drone_controller;
atomic_uint_fast8_t current_input_mode = INPUT_TYPE::ACRO;
atomic_uint_fast8_t current_input_mode = INPUT_TYPE::AUTO_NAV;
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) {
if (this->angvel.norm() < 1.0) {
this->angvel_stablilized = true;
} else {
dcont::set_input(drone_controller,
dcont::Input{.joystick = {.throttle_input = 0.0,
.roll_input = 0.0,
@ -74,27 +80,25 @@ struct drone_cont_state {
.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) {
if (this->vel.z() - 1.0 >= 0) {
this->fall_vel_stabilized = true;
} else {
dcont::set_input(drone_controller,
dcont::Input{.joystick = {.throttle_input = 1.0,
dcont::Input{.joystick = {.throttle_input = 0.6,
.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},
.rotation = {0.0, M_PI / 8, 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;
}
}
@ -114,20 +118,28 @@ struct drone_cont_state {
return this->fall_vel_stabilized && this->angvel_stablilized;
}
Eigen::Vector3f velocity_hist[25];
int velocity_hist_index = 0;
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;
this->rot = imu_state_local.rot;
}
if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
this->pos = sens_fus.position;
this->vel = sens_fus.velocity;
velocity_hist[velocity_hist_index] = sens_fus.velocity;
velocity_hist_index++;
velocity_hist_index = velocity_hist_index % 25;
xSemaphoreGive(sens_fus_mutex);
}
this->rot = imu_state_var.rot;
this->vel = Eigen::Vector3f::Zero();
for (int i = 0; i < 25; i++) {
this->vel += velocity_hist[i] / 25.0f;
}
}
void update_controller_sens() {
@ -136,7 +148,6 @@ struct drone_cont_state {
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(),
@ -147,81 +158,146 @@ struct drone_cont_state {
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;
bool input_was_set = false;
switch (atomic_load(&this->current_input_mode)) {
case INPUT_TYPE::ACRO: {
if (controller_input_semaphore &&
xSemaphoreTake(controller_input_semaphore, 10)) {
c = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
float ly = 0.6 * c.ly + 0.15;
auto inp = dcont::Input{.joystick = {.throttle_input = c.ly,
auto inp = dcont::Input{.joystick = {.throttle_input = ly,
.roll_input = c.rx,
.yaw_input = c.lx,
.pitch_input = c.ry},
.pitch_input = -c.ry},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {-c.ry, c.rx, c.lx},
.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};
dcont::set_input(drone_controller, inp);
this->last_input = inp;
input_was_set = true;
}
} break;
case INPUT_TYPE::LEVEL: {
if (controller_input_semaphore &&
xSemaphoreTake(controller_input_semaphore, 10)) {
c = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
float ly = 0.6 * c.ly + 0.15;
auto inp =
dcont::Input{.joystick = {.throttle_input = ly,
.roll_input = c.rx,
.yaw_input = c.lx,
.pitch_input = -c.ry},
.acceleration = {0.0, 0.0, 0.0},
.rotation = {-c.ry * 7 * 0.087f, c.rx * 7 * 0.087f,
c.lx * (float)M_PI},
.velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Rotation};
dcont::set_input(drone_controller, inp);
// ESP_LOGI("TEST", "(%f,%f), (%f,%f)", c.lx, c.ly, c.rx, c.ry);
} else {
ESP_LOGE("TAG", "ERRR");
this->last_input = inp;
input_was_set = true;
}
} 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();
//
// 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;
// }
case INPUT_TYPE::VELOCITY: {
if (controller_input_semaphore &&
xSemaphoreTake(controller_input_semaphore, 10)) {
c = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
auto inp = dcont::Input{
.joystick = {.throttle_input = 0.0,
.roll_input = 0.0,
.yaw_input = 0.0,
.pitch_input = 0.0},
.acceleration = {c.rx * 6.0f, c.ry * 6.0f, c.ly * 6.0f},
.rotation = {0.0, 0.0, 0.0},
.velocity = {c.rx * 5.0f, c.ry * 5.0f, c.ly * 5.0f},
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Acceleration};
dcont::set_input(drone_controller, inp);
this->last_input = inp;
input_was_set = true;
}
} 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();
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});
input_was_set = true;
}
} else {
drone_cont_stabilize();
}
break;
}
default:
case INPUT_TYPE::STABILIZE_FALL: {
drone_cont_stabilize();
break;
}
}
if (!input_was_set) {
if (atomic_load(&this->current_input_mode) != INPUT_TYPE::AUTO_NAV) {
auto inp = 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};
dcont::set_input(drone_controller, inp);
this->last_input = inp;
} else {
this->drone_cont_stabilize();
}
}
}
void update() {

9
main/env_sens.cpp
View File

@ -40,9 +40,10 @@ void setup() {
return;
}
ESP_LOGI(TAG, "BARO SETUP COMPLETE.");
bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X1,
Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_NONE,
Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_500);
bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X4,
Adafruit_BME280::SAMPLING_X4, Adafruit_BME280::SAMPLING_NONE,
Adafruit_BME280::FILTER_OFF,
Adafruit_BME280::STANDBY_MS_62_5);
bme_temp->printSensorDetails();
bme_pressure->printSensorDetails();
@ -142,7 +143,7 @@ void baro_poll_task(void *_) {
}
// BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
vTaskDelay(pdMS_TO_TICKS(500));
vTaskDelay(pdMS_TO_TICKS(100));
}
}

13
main/imu.cpp
View File

@ -1,4 +1,5 @@
#include "imu.h"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "Esp.h"
#include "drone_controller.h"
@ -45,15 +46,13 @@ BNO08x *setup_imu() {
}
imu->print_product_ids();
// imu->dynamic_calibration_run_routine();
imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all);
// 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.accelerometer.enable(10000UL);
imu->rpt.cal_gyro.enable(2500UL);
imu->rpt.rv.enable(10000UL); // 100Hz
imu->rpt.linear_accelerometer.enable(10000UL); // 100Hz
imu->rpt.cal_gyro.enable(2500UL); // 400Hz
imu->register_cb([imu, local_state]() {
// ESP_LOGI("IMU", "CALLBACK");
@ -69,6 +68,8 @@ BNO08x *setup_imu() {
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);
local_state->rot_euler =
Eigen::Vector3f(sens_euler.x, sens_euler.y, sens_euler.z);
// Eigen::Quaternionf q_global_yaw(
// Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));

92
main/main.cpp
View File

@ -23,15 +23,15 @@
#include "portmacro.h"
#include "radio.h"
#include "sens_fus.h"
#include "servo.h"
static const char *TAG = "MAIN";
#define TIME_RELEASE_QUEUE_TO_ACTIVATION 500
#define TIME_RELEASE_QUEUE_TO_ACTIVATION 1000
bool first_wayp_was_set = false;
extern "C" void app_main(void) {
sens_fus_mutex = xSemaphoreCreateMutex();
configASSERT(sens_fus_mutex);
nav_mutex = xSemaphoreCreateMutex();
initArduino();
@ -76,19 +76,6 @@ extern "C" void app_main(void) {
setup_imu();
vTaskDelay(10);
xTaskCreatePinnedToCore(motor_throttles_task, // Function name
"motor_throttles_task", // Name for debugging
1024 * 8, // Stack size in bytes
NULL, // Parameters
24, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
);
vTaskDelay(2);
xTaskCreatePinnedToCore(drone_controller_task, // Function name
"drone_controller_task", // Name for debugging
1024 * 32, // Stack size in bytes
@ -97,9 +84,8 @@ extern "C" void app_main(void) {
NULL, // Task handle
0 // Core ID
);
servo_init();
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
ESP_LOGI("MAIN", "FLASHED2");
Eigen::Vector3f local_pos = {0, 0, 0};
Eigen::Vector3f local_vel = {0, 0, 0};
@ -114,19 +100,24 @@ extern "C" void app_main(void) {
while (true) {
if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
if (millis() > last_position_broadcast_time + 100 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
last_position_broadcast_time = millis();
}
if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) {
if (millis() > last_status_broadcast_time + 5000 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS);
send_packet_getter(PACKET_TYPE::DRONE_NAV_WAYPOINT_0);
for (uint8_t i = 0; i < 8; i++) {
send_packet_getter(
(PACKET_TYPE)(PACKET_TYPE::DRONE_NAV_WAYPOINT_0 + i));
}
last_status_broadcast_time = millis();
}
if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) {
if (gps_mutex && xSemaphoreTake(gps_mutex, 10)) {
if (gps && gps->gps_avaliable()) {
waypoint current_wayp = nav_man.get_current_waypoint();
auto pos = sens_fus.position;
@ -134,19 +125,25 @@ extern "C" void app_main(void) {
if ((current_wayp.coords_in_axis.value_or(
Eigen::Vector3f(INFINITY, INFINITY, INFINITY)) -
pos)
.norm() < 1.0) {
.norm() < 2.0) {
nav_man.waypoint_reached();
}
}
xSemaphoreGive(gps_mutex);
}
xSemaphoreGive(nav_mutex);
}
// Release
if (imu_state_var.lin_accel_global.z < -8.0 && !released) {
if (xSemaphoreTake(imu_state_mutex, 1)) {
if (imu_state_var.lin_accel_global.z < -7.0 && !released) {
released = true;
time_activation_queue = millis();
}
xSemaphoreGive(imu_state_mutex);
}
if (released && drone_cont &&
std::atomic_load(&drone_cont->current_input_mode) ==
@ -155,16 +152,23 @@ extern "C" void app_main(void) {
!active) {
dcont::reset_pid_states(drone_cont->drone_controller);
active = true;
// servo_set(SERVO_OPTIONS::UP);
if (xSemaphoreTake(imu_state_mutex, 100)) {
if (imu_state_var.lin_accel_global.z < -7.0) {
active = true;
xTaskCreate(
[](void *pvParameters) {
vTaskDelay(pdMS_TO_TICKS(700));
vTaskDelay(pdMS_TO_TICKS(100));
std::atomic_store(&killswitch_active, false);
vTaskDelete(NULL);
},
"lambda_task", 2048, NULL, 5, NULL);
"lambda_task", 4096, NULL, 5, NULL);
} else {
released = false;
}
xSemaphoreGive(imu_state_mutex);
}
}
// Logging
@ -183,24 +187,31 @@ extern "C" void app_main(void) {
lon = gps->gps->longitudeDegrees;
alt = gps->gps->altitude;
if (!first_wayp_was_set) {
first_wayp_was_set = true;
xTaskCreate(
[](void *pvParameters) {
Eigen::Vector3f coords = Eigen::Vector3f::Zero();
vTaskDelay(10000);
int count = 0;
for (int i = 0; i < 10; i++) {
vTaskDelay(6000);
vTaskDelay(4000);
if (gps_mutex &&
xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
auto lat = gps->gps->latitudeDegrees;
auto lon = gps->gps->longitudeDegrees;
auto alt = gps->gps->altitude;
coords += Eigen::Vector3f(lat, lon, alt) * 0.1;
coords +=
Eigen::Vector3f(lat, lon, sens_fus.position.z());
count++;
xSemaphoreGive(gps_mutex);
} else {
ESP_LOGE("FIRST_WAYP", "FAILED TO GET MUTEX ON AVG");
}
}
coords = coords / (float)count;
if (gps_mutex &&
xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
@ -208,14 +219,26 @@ extern "C" void app_main(void) {
gps->origin_lat = coords.x();
gps->origin_long = coords.y();
xSemaphoreGive(gps_mutex);
} else {
ESP_LOGE("FIRST_WAYP", "FAILED TO GET MUTEX ON ORIGIN");
}
if (nav_mutex && xSemaphoreTake(nav_mutex, 1000)) {
nav_man.waypoints[0].coords = coords;
nav_man.current_waypoint = 0;
nav_man.waypoints[0].active = true;
nav_man.waypoints[0].landing = true;
xSemaphoreGive(nav_mutex);
} else {
ESP_LOGE("FIRST_WAYP", "FAILED TO GET NAV MUTEX");
first_wayp_was_set = false;
}
vTaskDelete(NULL);
},
"lambda_task_gps_init", 2048, NULL, 5, NULL); // FIXME: REMOVE
"lambda_task_gps_init", 8192, NULL, 5, NULL);
}
gps_values = true;
}
sat_count = gps->gps->satellites;
@ -266,13 +289,14 @@ extern "C" void app_main(void) {
motor_throttles[3], std::atomic_load(&killswitch_active));
}
if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) {
if (xSemaphoreTake(controller_input_semaphore, 10)) {
ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)",
current_controller_input.lx, current_controller_input.ly,
current_controller_input.rx, current_controller_input.ry);
xSemaphoreGive(controller_input_semaphore);
}
// 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());
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));

8
main/nav.h
View File

@ -49,21 +49,23 @@ struct drone_nav {
}
void waypoint_reached() {
int start_index = this->current_waypoint;
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;
this->current_waypoint = 8;
return;
}
int i = this->current_waypoint + 1;
while (i != this->current_waypoint) {
int i = start_index + 1;
while (i != start_index) {
bool active = waypoints[i].active;
if (active) {
this->current_waypoint = i;
return;
}
i++;
i = i % 8;

35
main/packet_handler.cpp
View File

@ -61,6 +61,7 @@ void handle_packet(uint8_t *packet_addr) {
atomic_store(&drone_cont->current_input_mode, packet->input_type);
} else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) {
// ESP_LOGI("RADIO_RX", "Controller recvd");
packet_controller_input *packet =
(packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
@ -68,6 +69,8 @@ void handle_packet(uint8_t *packet_addr) {
current_controller_input = *packet;
time_last_controller = millis();
xSemaphoreGive(controller_input_semaphore);
} else {
ESP_LOGE("RADIO_RX", "FAILED TO GET JOYSTICK");
}
}
}
@ -83,6 +86,8 @@ void handle_waypoint_update(uint8_t *packet_addr, uint8_t index) {
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
nav_man.waypoints[index].coords = coords;
nav_man.waypoints[index].landing = packet->landing;
nav_man.waypoints[index].active = packet->active;
xSemaphoreGive(nav_mutex);
}
}
@ -92,7 +97,6 @@ void handle_nav_update(uint8_t *packet_addr) {
(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;
}
}
@ -103,8 +107,10 @@ void send_packet_getter(PACKET_TYPE requested_type) {
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
Eigen::Vector3f local_vel = imu_state_var.rot.inverse() * sens_fus.velocity;
if (xSemaphoreTake(nav_mutex, 10)) {
if (xSemaphoreTake(imu_state_mutex, 10)) {
Eigen::Vector3f local_vel =
imu_state_var.rot.inverse() * sens_fus.velocity;
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
@ -113,7 +119,13 @@ void send_packet_getter(PACKET_TYPE requested_type) {
.vel = {local_vel.x(), local_vel.y(), local_vel.z()},
.rot = {imu_state_var.rot.w(), imu_state_var.rot.x(),
imu_state_var.rot.y(), imu_state_var.rot.z()},
.press = env_sens::get_pressure(),
.temp = env_sens::get_temperature(),
});
xSemaphoreGive(imu_state_mutex);
}
xSemaphoreGive(nav_mutex);
}
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
@ -131,14 +143,13 @@ void send_packet_getter(PACKET_TYPE requested_type) {
// Navigation
if (requested_type == PACKET_TYPE::DRONE_NAV) {
uint8_t active_mask, current;
uint8_t 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});
resp_packet = create_packet_pooled(PACKET_TYPE::DRONE_NAV,
packet_drone_nav{current});
}
}
@ -154,15 +165,19 @@ void send_packet_getter(PACKET_TYPE requested_type) {
Eigen::Vector3f coords;
bool land = false;
bool active = false;
if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
coords = nav_man.waypoints[index].coords;
land = nav_man.waypoints[index].landing;
xSemaphoreGive(nav_mutex);
}
active = nav_man.waypoints[index].active;
resp_packet = create_packet_pooled(
requested_type,
packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}, land});
packet_drone_nav_waypoint{.coord = {coords[0], coords[1], coords[2]},
.landing = land,
.active = active});
xSemaphoreGive(nav_mutex);
}
}
if (resp_packet.first != nullptr) {

5
main/radio.cpp
View File

@ -66,6 +66,8 @@ void radio_task(void *pvParameters) {
radio.readAllRegsCompact();
} else {
radio.readAllRegsCompact();
ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
ESP.restart();
}
@ -73,7 +75,7 @@ void radio_task(void *pvParameters) {
while (1) {
uint32_t now = millis();
if (radio.receiveDone()) {
while (radio.receiveDone()) {
// If we receive ANY valid packet while in probation, confirm the switch
// (Bit-rate switching)
@ -102,7 +104,6 @@ void radio_task(void *pvParameters) {
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);
}

6
main/sens_fus.h
View File

@ -46,11 +46,11 @@ 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.0};
Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY};
Eigen::Vector3f tau_gps_pos = {2.0f, 2.0f, 2.0};
Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, INFINITY};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 2.0f};
Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();

76
main/servo.cpp
View File

@ -1,8 +1,12 @@
#include "servo.h"
#include "driver/ledc.h"
#include "esp32-hal.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "hal/adc_types.h"
#include <cstdint>
#include <limits>
#define SERVO_PIN 7
#define LEDC_TIMER LEDC_TIMER_0
@ -50,3 +54,75 @@ void servo_set(SERVO_OPTIONS opt) {
ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
}
}
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"
#include "esp_adc/adc_oneshot.h"
#define LIGHT_ADC_CHANNEL ADC_CHANNEL_6
adc_cali_handle_t cali_handle = NULL;
adc_oneshot_unit_handle_t adc1_handle;
void adc_init() {
adc_oneshot_unit_init_cfg_t init_config = {.unit_id = ADC_UNIT_1};
adc_oneshot_new_unit(&init_config, &adc1_handle);
adc_oneshot_chan_cfg_t chan_cfg = {
.atten = ADC_ATTEN_DB_12, // 0-3.3V range
.bitwidth = ADC_BITWIDTH_12,
};
adc_oneshot_config_channel(adc1_handle, LIGHT_ADC_CHANNEL, &chan_cfg);
adc_cali_curve_fitting_config_t cali_config = {.unit_id = ADC_UNIT_1,
.atten = ADC_ATTEN_DB_12,
.bitwidth = ADC_BITWIDTH_12};
adc_cali_create_scheme_curve_fitting(&cali_config, &cali_handle);
}
int adc_read() {
int raw_val;
int voltage_mv;
adc_oneshot_read(adc1_handle, LIGHT_ADC_CHANNEL, &raw_val);
adc_cali_raw_to_voltage(cali_handle, raw_val, &voltage_mv);
return voltage_mv;
}
#define TIME_DARK_IN_ROCKET 30000
#define TIME_LIGHT_FOR_RELEASE 500
#define MAXIMUM_LIGHT_VOLTAGE 1000
bool has_been_in_rocket = false;
bool released = false;
constexpr uint64_t max_u64 = std::numeric_limits<uint64_t>::max();
uint64_t time_light_seen = max_u64;
uint64_t time_dark_seen = max_u64;
bool light_released() {
bool is_light = adc_read() < MAXIMUM_LIGHT_VOLTAGE;
if (is_light) {
if (has_been_in_rocket) {
if (time_light_seen == max_u64) {
time_light_seen = millis();
} else {
if (millis() - time_light_seen > TIME_LIGHT_FOR_RELEASE) {
released = true;
}
}
}
time_dark_seen = max_u64;
} else {
if (time_dark_seen == max_u64) {
time_light_seen = millis();
} else {
if (millis() - time_dark_seen > TIME_DARK_IN_ROCKET) {
has_been_in_rocket = true;
}
}
time_light_seen = max_u64;
}
}

3
main/servo.h
View File

@ -9,3 +9,6 @@ enum SERVO_OPTIONS {
void servo_init();
void servo_set(SERVO_OPTIONS opt);
void adc_init();
int adc_read();