From 4437f498ae59f955947bb7ae449bc2895f142d03 Mon Sep 17 00:00:00 2001 From: franchioping Date: Mon, 13 Apr 2026 14:36:24 +0100 Subject: [PATCH] broken but fixable --- main/drone.cpp | 67 ++++++++++++++++++++++++++++++++------------------ main/drone.h | 3 ++- main/main.cpp | 12 +++++---- main/radio.cpp | 10 ++++---- 4 files changed, 57 insertions(+), 35 deletions(-) diff --git a/main/drone.cpp b/main/drone.cpp index 5545b63..7af0f94 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -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]->sendThrottlePercent(motor_throttles[i] * 100.0f); - } else { - motors[i]->sendThrottlePercent(0.0f); - } + motors[i]->begin(); + + motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f); + + rmt_tx_wait_all_done(motors[i]->_rmt_tx_channel, 2); + // delayMicroseconds(50); + motors[i]->_cleanupRmtResources(); } - vTaskDelay(1); + // vTaskDelay(1); } } diff --git a/main/drone.h b/main/drone.h index 0005ce8..42a715f 100644 --- a/main/drone.h +++ b/main/drone.h @@ -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; diff --git a/main/main.cpp b/main/main.cpp index 7984015..de0de9b 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -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}; diff --git a/main/radio.cpp b/main/radio.cpp index 1b9aae8..6325a9c 100644 --- a/main/radio.cpp +++ b/main/radio.cpp @@ -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