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

bare minimum to fly with a controller. initial navigation development. TODO: mark waypoints as inactive and continue to next when current one is reached, drone stabilization logic.

This commit is contained in:
franchioping 2026-04-06 00:27:32 +01:00
parent b315af0f66
commit bd81652515
10 changed files with 192 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_controller

@ -1 +1 @@
Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2
Subproject commit ac127154714cf9ce781757d7203651fed214b5ff

83
main/drone.cpp
View File

@ -1,17 +1,26 @@
#include "drone.h"
#include "DShotRMT.h"
#include "Eigen/Core"
#include "drone_comms.h"
#include "drone_controller.h"
#include "dshot_definitions.h"
#include "esp32-hal.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 <cstdint>
#include <cstring>
#include <optional>
#define CONTROLLER_TASK_FREQUENCY 400.0;
#define CONTROLLER_TASK_FREQUENCY 1000.0;
dcont::ControllerConfig default_config() {
dcont::ControllerConfig config;
@ -39,7 +48,7 @@ dcont::ControllerConfig default_config() {
config.stack.rate_pid = {{0.2f, 0.2f, 2.0f},
{0.05f, 0.05f, 0.05f},
{0.003f, 0.003f, 0.001f},
600.0f};
1000.0f};
// 2. Set Constraints
config.stack.max_rate = 3.14f; // ~180 degrees/s
@ -69,6 +78,13 @@ dcont::ControllerConfig default_config() {
void setup_drone() { drone_controller = dcont::create(default_config()); }
void drone_cont_stabilize() {
// TODO: Implement stabilization. if |angvel| > something, we should make it 0
// first, then if falling fast, pull rotation to the side, then when falling
// is stabilized, turn slowly up until pointing up. let velocity dissipate
// afterwards
}
void drone_controller_task(void *params) {
setup_drone();
uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
@ -94,6 +110,69 @@ void drone_controller_task(void *params) {
dcont::set_cur_pos(drone_controller, v3f_to_v3c(position_local));
dcont::set_cur_rot(drone_controller, imu_state_local.rot);
packet_controller_input cont_input;
if (current_input_mode == dcont::ModeInput::Acro &&
xSemaphoreTake(controller_input_semaphore, 10)) {
cont_input = current_controller_input;
xSemaphoreGive(controller_input_semaphore);
}
waypoint wayp;
if (current_input_mode == dcont::ModeInput::Position &&
xSemaphoreTake(nav_mutex, 10)) {
wayp = nav_man.get_current_waypoint();
xSemaphoreGive(nav_mutex);
}
if (current_input_mode == dcont::ModeInput::Position &&
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{{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});
}
memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
sizeof(motor_throttles));
vTaskDelay(pdMS_TO_TICKS(wait_ms));
}
}
const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC,
GPIO_NUM_NC};
void motor_throttles_task(void *params) {
DShotRMT *motors[4];
for (int i = 0; i < 4; i++) {
motors[i] = new DShotRMT(motor_pins[i], DSHOT150, false);
}
// ARM
unsigned long armTime = millis();
while (millis() - armTime < 4000) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(0);
}
}
while (true) {
for (int i = 0; i < 4; i++) {
if (!killswitch_active) {
motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f);
} else {
motors[i]->sendThrottlePercent(0.0f);
}
}
vTaskDelay(1);
}
}

5
main/drone.h
View File

@ -22,3 +22,8 @@ inline dcont::StackedController *drone_controller = nullptr;
inline dcont::ModeInput current_input_mode;
void drone_controller_task(void *params);
void motor_throttles_task(void *params);
inline float motor_throttles[4];
inline bool killswitch_active;

5
main/gps.h
View File

@ -19,7 +19,6 @@
const float TO_RAD = M_PI / 180.0f;
const float KNOTS_TO_M_SEC = 0.5144444f;
const float earth_radius = 6371000.0f;
struct GPS {
@ -73,8 +72,8 @@ struct GPS {
}
std::optional<Eigen::Vector3f> get_coordinates() {
if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 ||
this->gps->longitudeDegrees != 0.0) {
if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 &&
this->gps->longitudeDegrees == 0.0)) {
return std::nullopt;
}
float latitude = this->gps->latitudeDegrees;

2
main/imu.cpp
View File

@ -33,7 +33,7 @@ void setup_imu() {
imu->rpt.rv_game.enable(2500UL);
imu->rpt.linear_accelerometer.enable(2500UL);
imu->rpt.cal_gyro.enable(2500UL);
imu->rpt.cal_gyro.enable(1000UL);
imu->register_cb([imu, local_state]() {
bool needs_updating = false;

22
main/main.cpp
View File

@ -44,6 +44,10 @@ extern "C" void app_main(void) {
0 // Core ID
);
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
xTaskCreatePinnedToCore(drone_controller_task, // Function name
"drone_controller_task", // Name for debugging
1024 * 32, // Stack size in bytes
@ -53,20 +57,29 @@ extern "C" void app_main(void) {
1 // Core ID
);
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
xTaskCreatePinnedToCore(motor_throttles_task, // Function name
"motor_throttles_task", // Name for debugging
1024 * 4, // Stack size in bytes
NULL, // Parameters
20, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
);
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;
while (true) {
while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
handle_packet(&packet_data[0]);
}
if (millis() > last_print_time + 1000) {
std::optional<Eigen::Vector3f> coords;
float lat, lon, alt;
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
@ -102,7 +115,8 @@ extern "C" void app_main(void) {
ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
local_vel[2]);
}
}
vTaskDelay(pdMS_TO_TICKS(1000));
vTaskDelay(pdMS_TO_TICKS(10));
}
}

18
main/nav.h
View File

@ -2,7 +2,10 @@
#include "Eigen/Core"
#include "freertos/idf_additions.h"
#include "gps.h"
#include <cmath>
#include <cstdint>
#include <optional>
#ifdef PS
#undef PS
@ -17,8 +20,9 @@
#define WAYPOINT_COUNT 8
struct waypoint {
Eigen::Vector3f coords; // lon, lat, alt
bool active; // active or to be skipped
Eigen::Vector3f coords; // lat, lon, alt
std::optional<Eigen::Vector3f> coords_in_axis = std::nullopt;
bool active = false; // active or to be skipped
};
struct drone_nav {
@ -40,6 +44,16 @@ struct drone_nav {
}
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;
}
};
inline SemaphoreHandle_t nav_mutex = NULL;

34
main/packet_handler.cpp
View File

@ -1,7 +1,10 @@
#include "packet_handler.h"
#include "Eigen/Core"
#include "drone.h"
#include "drone_comms.h"
#include "drone_controller.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "gps.h"
#include "nav.h"
@ -32,13 +35,38 @@ void handle_packet(uint8_t *packet_addr) {
}
// NAV SETTERS
if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
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);
}
if (packet_type == PACKET_TYPE::DRONE_NAV) {
} 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));
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));
switch (packet->input_type) {
case INPUT_TYPE::ACRO:
current_input_mode = dcont::ModeInput::Acro;
break;
case INPUT_TYPE::LEVEL:
case INPUT_TYPE::STABILIZE_FALL:
ESP_LOGE("PACKET_HANDLER",
"INPUT TYPES NOT IMPLEMENTED. DEFAULTING TO POSITION (AUTONAV)");
case INPUT_TYPE::AUTO_NAV:
current_input_mode = dcont::ModeInput::Position;
}
} 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;
xSemaphoreGive(controller_input_semaphore);
}
}
}

6
main/packet_handler.h
View File

@ -2,4 +2,10 @@
#include <cstdint>
#include "drone_comms.h"
#include "freertos/idf_additions.h"
void handle_packet(uint8_t *packet_addr);
inline SemaphoreHandle_t controller_input_semaphore = NULL;
inline packet_controller_input current_controller_input;

2
main/radio.h
View File

@ -5,7 +5,5 @@
inline QueueHandle_t packet_rx_queue = NULL;
inline QueueHandle_t packet_tx_queue = NULL;
inline SemaphoreHandle_t controller_input_semaphore = NULL;
inline packet_controller_input current_controller_input;
void radio_task(void *pvParameters);