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

broken but fixable

This commit is contained in:
franchioping 2026-04-13 14:36:24 +01:00
parent 5f9bb289cd
commit 4437f498ae
4 changed files with 57 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
main/drone.cpp
View File

@ -2,6 +2,7 @@
#include "DShotRMT.h"
#include "Eigen/Core"
#include "driver/rmt_tx.h"
#include "drone_comms.h"
#include "drone_controller.h"
@ -33,23 +34,22 @@ dcont::ControllerConfig default_config() {
{1.0f, 1.0f, 1.0f}, // kp
{0.0f, 0.0f, 0.0f}, // ki
{0.0f, 0.0f, 0.0f}, // kd
25.0f // frequency (Hz)
5.0f // frequency (Hz)
};
// Velocity Loop (Velocity -> Acceleration/Tilt)
config.stack.linvel_pid = {
{1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 50.0f};
{1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 15.0f};
// Rotation Loop (Angle -> Angular Rate)
config.stack.rotation_pid = {
{4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 200.0f};
{4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 50.0f};
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
config.stack.rate_pid = {{0.2f, 0.2f, 2.0f},
{0.05f, 0.05f, 0.05f},
{0.003f, 0.003f, 0.001f},
1000.0f};
config.stack.rate_pid = {{0.1f, 0.1f, 1.0f},
{0.01f, 0.01f, 0.01f},
{0.001f, 0.001f, 0.001f},
100.0f};
// 2. Set Constraints
config.stack.max_rate = 3.14f; // ~180 degrees/s
config.stack.max_linvel = 10.0f; // 10 m/s
@ -58,13 +58,19 @@ dcont::ControllerConfig default_config() {
config.mass = 0.350f; // kg
config.max_thrust = 2.6f; // Newtons
config.max_torque = 0.5f; // Nm
// roll, pitch, yaw for each motor
float mixer[4][3] = {
/*
* roll, pitch, yaw
{1.0f, -1.0f, 1.0f}, // Front Right
{-1.0f, -1.0f, -1.0f}, // Front Left
{-1.0f, 1.0f, 1.0f}, // Rear Left
{1.0f, 1.0f, -1.0f} // Rear Right
*/
float mixer[4][3] = {
{1.0f, -1.0f, 1.0f}, // Front Right - Real front
{-1.0f, -1.0f, -1.0f}, // Front Left - Real left
{-1.0f, 1.0f, 1.0f}, // Rear Left - Real rear
{1.0f, 1.0f, -1.0f} // Rear Right - Real right
};
for (int i = 0; i < 4; i++) {
@ -148,34 +154,47 @@ void drone_controller_task(void *params) {
}
}
const gpio_num_t motor_pins[4] = {GPIO_NUM_14, GPIO_NUM_15, GPIO_NUM_16,
GPIO_NUM_46};
const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
GPIO_NUM_15};
const bool reversed[4] = {false, true, false, false};
DShotRMT *motors[4];
void motor_throttles_task(void *params) {
DShotRMT *motors[4];
for (int i = 0; i < 4; i++) {
motors[i] = new DShotRMT(motor_pins[i], DSHOT150, false);
motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
motors[i]->begin();
rmt_tx_wait_all_done(motors[i]->_rmt_tx_channel, 2);
motors[i]->_cleanupRmtResources();
}
motors[0]->sendCommand(3);
// ARM
unsigned long armTime = millis();
while (millis() - armTime < 5000) {
for (int i = 0; i < 4; i++) {
motors[i]->begin();
motors[i]->sendThrottlePercent(0);
rmt_tx_wait_all_done(motors[i]->_rmt_tx_channel, 2);
// delayMicroseconds(50);
motors[i]->_cleanupRmtResources();
}
vTaskDelay(1);
// vTaskDelay(1);
}
uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
while (true) {
for (int i = 0; i < 4; i++) {
if (!killswitch_active) {
motors[i]->begin();
motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f);
} else {
motors[i]->sendThrottlePercent(0.0f);
rmt_tx_wait_all_done(motors[i]->_rmt_tx_channel, 2);
// delayMicroseconds(50);
motors[i]->_cleanupRmtResources();
}
}
vTaskDelay(1);
// vTaskDelay(1);
}
}

3
main/drone.h
View File

@ -24,5 +24,6 @@ void drone_controller_task(void *params);
void motor_throttles_task(void *params);
inline float motor_throttles[4] = {0.05, 0.05, 0.05, 0.05};
// 3 4 2 1
inline float motor_throttles[4] = {0.01, 0.01, 0.01, 0.01};
inline bool killswitch_active = false;

12
main/main.cpp
View File

@ -38,11 +38,13 @@ extern "C" void app_main(void) {
// NULL, // Task handle
// 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(env_sens::baro_poll_task, "baro_poll", 8192, NULL,
// 1,
// 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
@ -56,14 +58,14 @@ extern "C" void app_main(void) {
"motor_throttles_task", // Name for debugging
1024 * 4, // Stack size in bytes
NULL, // Parameters
30, // Priority (higher = more urgent)
24, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
);
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
setup_imu();
// setup_imu();
Eigen::Vector3f local_pos = {0, 0, 0};
Eigen::Vector3f local_vel = {0, 0, 0};

10
main/radio.cpp
View File

@ -15,12 +15,12 @@
// Right to left on hardware.
#define RFM69_MOSI GPIO_NUM_11
#define RFM69_SCLK GPIO_NUM_12
#define RFM69_MOSI GPIO_NUM_12
#define RFM69_SCLK GPIO_NUM_11 // SCK
#define RFM69_MISO GPIO_NUM_13
#define RFM69_CS GPIO_NUM_10
#define RFM69_INT GPIO_NUM_9
#define RFM69_RST GPIO_NUM_8
#define RFM69_CS GPIO_NUM_9 // NSS
#define RFM69_INT GPIO_NUM_8 // DI0
#define RFM69_RST GPIO_NUM_10
#define FREQUENCY RF69_433MHZ
#define NODEID 1