working acro

components/drone_comms

@@ -1 +1 @@
-Subproject commit bb38f95e0de4868b2f84b3fa8ab3025f3496e37b
+Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20

main/drone.cpp

@@ -18,7 +18,9 @@
 #include "nav.h"
 #include "packet_handler.h"
 #include "sens_fus.h"
-#include "soc/gpio_num.h" #include < cstdint>
+#include "soc/gpio_num.h"
+#include <algorithm>
+#include <cstdint>
 #include <cstring>
 #include <optional>
 #include <stdlib.h>
@@ -65,11 +67,12 @@ dcont::ControllerConfig default_config() {
   config.max_torque = 0.5f; // Nm
 
   float mixer[4][3] = {
-      // roll, pitch, yaw
+      // x, y, z
-      {-1.0, -1.0, -1.0}, // Front Left
+
-      {1.0, 1.0, -1.0},   // Rear Right
+      {-1.0, -1.0, -1.0}, // Rear Right
       {-1.0, 1.0, 1.0},   // Rear Left
       {1.0, -1.0, 1.0},   // Front Right
+      {1.0, 1.0, -1.0},   // Front Left
   };
 
   for (int i = 0; i < 4; i++) {
@@ -90,13 +93,16 @@ void drone_controller_task(void *params) {
   while (true) {
     drone_cont->update();
 
-    vTaskDelay(pdMS_TO_TICKS(wait_ms));
+    vTaskDelay(pdMS_TO_TICKS(1));
   }
 }
 
 const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
                                   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];
 void motor_throttles_task(void *params) {
   motor_throttles = (float *)malloc(sizeof(float) * 4);
@@ -118,7 +124,8 @@ void motor_throttles_task(void *params) {
 
   while (true) {
     for (int i = 0; i < 4; i++) {
-      float throttle = motor_throttles[i] * 100.0f * 0.7f;
+      float throttle =
+          std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f;
       if (atomic_load(&killswitch_active)) {
         throttle = 0.0;
       }

main/drone.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "esp_log.h"
 #include "nav.h"
 
 #include "esp32-hal.h"
@@ -142,8 +143,11 @@ struct drone_cont_state {
   }
 
   void update_throttles() {
-    memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
+    auto thrt_var = dcont::get_throttles(drone_controller);
-           sizeof(float) * 4);
+    auto thrt = thrt_var.values;
+    memcpy(motor_throttles, thrt, sizeof(float) * 4);
+    // ESP_LOGI("CONT", "thr: %f, %f, %f, %f", thrt[0], thrt[1], thrt[2],
+    // thrt[3]); ESP_LOGE("CONT", "UPDATE THROTTLES");
   }
 
   void update_input() {
@@ -153,9 +157,9 @@ struct drone_cont_state {
     switch (atomic_load(&this->current_input_mode)) {
     case INPUT_TYPE::ACRO: {
       if (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};
-        }
+        // }
         cont_input = current_controller_input;
 
         xSemaphoreGive(controller_input_semaphore);

main/imu.cpp

@@ -89,6 +89,7 @@ BNO08x *setup_imu() {
 
       auto cal_gyro = imu->rpt.cal_gyro.get();
       local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z};
+      // ESP_LOGI("ROT", "angvel_z: %f", cal_gyro.z);
     }
 
     if (imu->rpt.linear_accelerometer.has_new_data()) {

main/main.cpp

@@ -9,8 +9,7 @@
 #include "freertos/projdefs.h"
 #include "freertos/task.h"
 #include <atomic>
-#include <cmath>
+#include <cmath> #include <cstdint>
-#include <cstdint>
 #include <optional>
 
 #include "env_sens.h"
@@ -50,14 +49,14 @@ extern "C" void app_main(void) {
 
   xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
 
-  // xTaskCreatePinnedToCore(drone_controller_task,   // Function name
+  xTaskCreatePinnedToCore(drone_controller_task,   // Function name
-  //                         "drone_controller_task", // Name for debugging
+                          "drone_controller_task", // Name for debugging
-  //                         1024 * 32,               // Stack size in bytes
+                          1024 * 32,               // Stack size in bytes
-  //                         NULL,                    // Parameters
+                          NULL,                    // Parameters
-  //                         20,   // Priority (higher = more urgent)
+                          20,   // Priority (higher = more urgent)
-  //                         NULL, // Task handle
+                          NULL, // Task handle
-  //                         1     // Core ID
+                          1     // Core ID
-  // );
+  );
 
   xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
                           "motor_throttles_task", // Name for debugging
@@ -68,6 +67,25 @@ extern "C" void app_main(void) {
                           1     // 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();
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
@@ -83,10 +101,6 @@ extern "C" void app_main(void) {
   bool released = false;
 
   while (true) {
-    while (packet_rx_queue &&
-           xQueueReceive(packet_rx_queue, &packet_data[0], 20)) {
-      handle_packet(&packet_data[0]);
-    }
 
     if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
       send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);

main/packet_handler.cpp

@@ -113,8 +113,7 @@ void send_packet_getter(PACKET_TYPE requested_type) {
             .vel = {local_vel.x(), local_vel.y(), local_vel.z()},
             .rot = {imu_state_var.rot.w(), imu_state_var.rot.x(),
                     imu_state_var.rot.y(), imu_state_var.rot.z()},
-            .angvel = {imu_state_var.angvel.x(), imu_state_var.angvel.y(),
+        });
-                       imu_state_var.angvel.z()}});
   }
 
   if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {