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

working yaw

This commit is contained in:
franchioping 2026-04-20 02:52:05 +01:00
parent 16ae08dafd
commit 435dfd4205
4 changed files with 93 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flake.nix
View File

@ -25,8 +25,15 @@
devShells = forEachSupportedSystem ({ pkgs }: { devShells = forEachSupportedSystem ({ pkgs }: {
default = pkgs.mkShell.override { } { default = pkgs.mkShell.override { } {
packages = with pkgs; packages = with pkgs;
[ esp-idf-full rustup openssl stdenv.cc.cc.lib libclang ] [
++ (if system == "aarch64-darwin" then [ ] else [ gdb ]); esp-idf-full
rustup
openssl
stdenv.cc.cc.lib
libclang
(pkgs.python3.withPackages
(python-pkgs: with python-pkgs; [ pandas matplotlib ]))
] ++ (if system == "aarch64-darwin" then [ ] else [ gdb ]);
shellHook = '' shellHook = ''
export CLANGD_QUERY_DRIVER="$(which clang),$(which clang++)" export CLANGD_QUERY_DRIVER="$(which clang),$(which clang++)"
export CMAKE_EXPORT_COMPILE_COMMANDS=1 export CMAKE_EXPORT_COMPILE_COMMANDS=1

54
main/drone.cpp
View File

@ -2,14 +2,14 @@
#include "DShotRMT.h" #include "DShotRMT.h"
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry" #include "driver/rmt_tx.h"
#include "driver/rmt_tx.h"
#include "drone_comms.h" #include "drone_comms.h"
#include "drone_controller.h" #include "drone_controller.h"
#include "dshot_definitions.h" #include "dshot_definitions.h"
#include "esp32-hal.h" #include "esp32-hal.h"
#include "esp_log.h" #include "esp_log.h"
#include "esp_log_timestamp.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h" #include "freertos/idf_additions.h"
#include "freertos/projdefs.h" #include "freertos/projdefs.h"
@ -25,7 +25,7 @@
#include <optional> #include <optional>
#include <stdlib.h> #include <stdlib.h>
#define CONTROLLER_TASK_FREQUENCY 1000.0; #define CONTROLLER_TASK_FREQUENCY 500.0;
dcont::ControllerConfig default_config() { dcont::ControllerConfig default_config() {
dcont::ControllerConfig config; dcont::ControllerConfig config;
@ -46,17 +46,24 @@ dcont::ControllerConfig default_config() {
.ki = {0.01f, 0.01f, 0.01f}, .ki = {0.01f, 0.01f, 0.01f},
.kd = {0.0f, 0.0f, 0.0f}, .kd = {0.0f, 0.0f, 0.0f},
.frequency = 50.0f}; .frequency = 50.0f};
// Rotation Loop (Rotation/Accel -> Angular Rate) // Rotation Loop (Rotation/Accel -> Angular Rate)
config.stack.rotation_pid = {.kp = {4.0f, 4.0f, 4.0f}, config.stack.rotation_pid = {
.ki = {1.0f, 1.0f, 1.0f}, .kp = {1.0f, 1.0f, 2.0f},
.kd = {0.0f, 0.0f, 0.0f}, .ki = {0.01f, 0.01f, 0.02f},
.frequency = 200.0f}; .kd = {0.1f, 0.1f, 0.0f},
.integral_cap = {4.0f, 4.0f, 4.0f},
.frequency = 200.0f,
};
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
config.stack.rate_pid = {.kp = {0.1f, 0.1f, 1.0f}, config.stack.rate_pid = {
.ki = {0.01f, 0.01f, 0.01f}, .kp = {0.0f, 0.0f, 1.0f},
.kd = {0.001f, 0.001f, 0.001f}, .ki = {0.00f, 0.00f, 0.0f},
.frequency = 1000.0f}; .kd = {0.000f, 0.000f, 0.0f},
.integral_cap = {1.0f, 1.0f, 1.0f},
.frequency = 500.0f,
};
// 2. Set Constraints // 2. Set Constraints
config.stack.max_rate = 3.14f; // ~180 degrees/s config.stack.max_rate = 3.14f; // ~180 degrees/s
config.stack.max_linvel = 3.0f; // 10 m/s config.stack.max_linvel = 3.0f; // 10 m/s
@ -92,6 +99,12 @@ void drone_controller_task(void *params) {
while (true) { while (true) {
drone_cont->update(); drone_cont->update();
// char *csv_str = (char
// *)dcont::debug_stacked(drone_cont->drone_controller,
// drone_cont->last_input);
//
// ESP_LOGI("DCONT_DBG", "START%sEND", csv_str);
// free(csv_str);
vTaskDelay(pdMS_TO_TICKS(1)); vTaskDelay(pdMS_TO_TICKS(1));
} }
@ -100,15 +113,12 @@ void drone_controller_task(void *params) {
const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14, const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
GPIO_NUM_15}; 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]; DShotRMT *motors[4];
void motor_throttles_task(void *params) { void motor_throttles_task(void *params) {
motor_throttles = (float *)malloc(sizeof(float) * 4); motor_throttles = (float *)malloc(sizeof(float) * 4);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
motor_throttles[i] = 0.02; motor_throttles[i] = 0.0;
motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false); motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
motors[i]->begin(); motors[i]->begin();
} }
@ -121,15 +131,25 @@ void motor_throttles_task(void *params) {
} }
vTaskDelay(2); vTaskDelay(2);
} }
dcont::reset_pid_states(drone_cont->drone_controller);
int i = 0;
while (true) { while (true) {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
float throttle = float throttle =
std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f; std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.4f;
if (atomic_load(&killswitch_active)) { if (atomic_load(&killswitch_active)) {
throttle = 0.0; throttle = 0.0;
} }
motors[i]->sendThrottlePercent(5.0); motors[i]->sendThrottlePercent(throttle);
}
i++;
if (i >= 50) {
ESP_LOGI("MOTORSSS", "throttles (%f,%f,%f,%f)", motor_throttles[0],
motor_throttles[1], motor_throttles[2], motor_throttles[3]);
i = 0;
} }
vTaskDelay(2); vTaskDelay(2);
} }

28
main/drone.h
View File

@ -48,6 +48,7 @@ dcont::ControllerConfig default_config();
struct drone_cont_state { struct drone_cont_state {
bool angvel_stablilized; bool angvel_stablilized;
bool fall_vel_stabilized; bool fall_vel_stabilized;
dcont::Input last_input;
Eigen::Vector3f pos; Eigen::Vector3f pos;
Eigen::Vector3f vel; Eigen::Vector3f vel;
@ -156,7 +157,8 @@ struct drone_cont_state {
switch (atomic_load(&this->current_input_mode)) { switch (atomic_load(&this->current_input_mode)) {
case INPUT_TYPE::ACRO: { case INPUT_TYPE::ACRO: {
if (xSemaphoreTake(controller_input_semaphore, 10)) { if (controller_input_semaphore &&
xSemaphoreTake(controller_input_semaphore, 10)) {
// if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) { // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) {
// current_controller_input = {0, 0, 0, 0}; // current_controller_input = {0, 0, 0, 0};
// } // }
@ -164,17 +166,19 @@ struct drone_cont_state {
xSemaphoreGive(controller_input_semaphore); xSemaphoreGive(controller_input_semaphore);
dcont::set_input( auto inp = dcont::Input{.joystick = {.throttle_input = 0.5,
drone_controller, .roll_input = cont_input.rx,
dcont::Input{.joystick = {.throttle_input = cont_input.ly, .yaw_input = cont_input.lx,
.roll_input = cont_input.rx, .pitch_input = cont_input.ry},
.yaw_input = cont_input.lx, .acceleration = {0.0, 0.0, 0.0},
.pitch_input = cont_input.ry}, .rotation = {0.0, 0.0, 0.0},
.acceleration = {0.0, 0.0, 0.0}, .velocity = {0.0, 0.0, 0.0},
.rotation = {1.0, 0.0, 0.0}, .position = {0.0, 0.0, 0.0},
.velocity = {0.0, 0.0, 0.0}, .mode = dcont::ModeInput::Rotation};
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Rotation}); dcont::set_input(drone_controller, inp);
this->last_input = inp;
} else { } else {
drone_cont_stabilize(); drone_cont_stabilize();
} }

57
main/main.cpp
View File

@ -3,13 +3,15 @@
#include "drone.h" #include "drone.h"
#include "drone_comms.h" #include "drone_comms.h"
#include "drone_controller.h" #include "drone_controller.h"
#include "esp32-hal.h" #include "esp_log.h" #include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h" #include "freertos/idf_additions.h"
#include "freertos/projdefs.h" #include "freertos/projdefs.h"
#include "freertos/task.h" #include "freertos/task.h"
#include <atomic> #include <atomic>
#include <cmath> #include <cstdint> #include <cmath>
#include <cstdint>
#include <optional> #include <optional>
#include "env_sens.h" #include "env_sens.h"
@ -49,6 +51,29 @@ extern "C" void app_main(void) {
xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
// 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(10);
}
},
"lambda_recv_task", 8192, NULL, 5, NULL);
setup_imu();
vTaskDelay(10);
xTaskCreatePinnedToCore(motor_throttles_task, // Function name xTaskCreatePinnedToCore(motor_throttles_task, // Function name
"motor_throttles_task", // Name for debugging "motor_throttles_task", // Name for debugging
1024 * 4, // Stack size in bytes 1024 * 4, // Stack size in bytes
@ -68,27 +93,6 @@ extern "C" void app_main(void) {
NULL, // Task handle NULL, // Task handle
0 // Core ID 0 // 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(); servo_init();
ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
@ -120,7 +124,7 @@ extern "C" void app_main(void) {
if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) { if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) {
if (gps->gps_avaliable()) { if (gps && gps->gps_avaliable()) {
waypoint current_wayp = nav_man.get_current_waypoint(); waypoint current_wayp = nav_man.get_current_waypoint();
auto pos = sens_fus.position; auto pos = sens_fus.position;
@ -211,8 +215,9 @@ extern "C" void app_main(void) {
if (motor_throttles != nullptr) { if (motor_throttles != nullptr) {
ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0], ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f), kill: %d",
motor_throttles[1], motor_throttles[2], motor_throttles[3]); motor_throttles[0], motor_throttles[1], motor_throttles[2],
motor_throttles[3], std::atomic_load(&killswitch_active));
} }
if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) { if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) {