broken but fixable

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},
+  config.stack.rate_pid = {{0.1f, 0.1f, 1.0f},
-                           {0.05f, 0.05f, 0.05f},
+                           {0.01f, 0.01f, 0.01f},
-                           {0.003f, 0.003f, 0.001f},
+                           {0.001f, 0.001f, 0.001f},
-                           1000.0f};
+                           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
+    * roll, pitch, yaw
-  float mixer[4][3] = {
       {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,
+const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
-                                  GPIO_NUM_46};
+                                  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(motor_throttles[i] * 100.0f);
-        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);
   }
 }

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;

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};

main/radio.cpp

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