drone task inputs. added drone.h and drone.cpp

components/drone_controller

@@ -1 +1 @@
-Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502
+Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2

main/drone.cpp

@@ -0,0 +1,99 @@
+#include "drone.h"
+
+#include "drone_controller.h"
+
+#include "esp32-hal.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
+#include "freertos/projdefs.h"
+#include "freertos/task.h"
+#include "imu.h"
+#include "sens_fus.h"
+#include <cstdint>
+
+#define CONTROLLER_TASK_FREQUENCY 400.0;
+
+dcont::ControllerConfig default_config() {
+  dcont::ControllerConfig config;
+
+  // 1. Configure the PID Stack
+  // Note: kp, ki, kd are arrays of 3 [roll, pitch, yaw]
+
+  // Position Loop (Position -> Velocity)
+  config.stack.position_pid = {
+      {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)
+  };
+
+  // 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};
+
+  // 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};
+
+  // 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},
+                           600.0f};
+
+  // 2. Set Constraints
+  config.stack.max_rate = 3.14f;   // ~180 degrees/s
+  config.stack.max_linvel = 10.0f; // 10 m/s
+
+  // 3. Physical Drone Properties
+  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] = {
+      {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
+  };
+
+  for (int i = 0; i < 4; i++) {
+    for (int j = 0; j < 3; j++) {
+      config.motor_map[i][j] = mixer[i][j];
+    }
+  }
+
+  return config;
+}
+
+void setup_drone() { drone_controller = dcont::create(default_config()); }
+
+void drone_controller_task(void *params) {
+  setup_drone();
+  uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
+
+  imu_state imu_state_local;
+  Eigen::Vector3f position_local = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero();
+
+  while (true) {
+    if (xSemaphoreTake(imu_state_mutex, 1)) {
+      imu_state_local = imu_state_var;
+      xSemaphoreGive(imu_state_mutex);
+    }
+    if (xSemaphoreTake(sens_fus_mutex, 1)) {
+      position_local = sens_fus.position;
+      velocity_local = sens_fus.velocity;
+      xSemaphoreGive(sens_fus_mutex);
+    }
+
+    dcont::set_cur_time(drone_controller, millis() / 1000.0f);
+    dcont::set_cur_angvel(drone_controller, v3f_to_v3c(imu_state_local.angvel));
+    dcont::set_cur_linvel(drone_controller, v3f_to_v3c(velocity_local));
+    dcont::set_cur_pos(drone_controller, v3f_to_v3c(position_local));
+    dcont::set_cur_rot(drone_controller, imu_state_local.rot);
+
+    vTaskDelay(pdMS_TO_TICKS(wait_ms));
+  }
+}

main/drone.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "Eigen/Core"
+#include "drone_comms.h"
+#include "drone_controller.h"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+void setup_drone();
+
+inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; }
+
+inline dcont::StackedController *drone_controller = nullptr;
+inline dcont::ModeInput current_input_mode;
+
+void drone_controller_task(void *params);

main/imu.cpp

@@ -68,7 +68,8 @@ void setup_imu() {
 
         float dt =
             ((float)(current_time - local_state->last_time)) / 1000000.0f;
-        Vec3C accel_global = apply_rot(&local_state->accel, &local_state->rot);
+        dcont::Vec3C accel_global =
+            dcont::apply_rot(&local_state->accel, &local_state->rot);
 
         if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
           sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,

main/imu.h

@@ -25,8 +25,8 @@
 #include <Eigen/Dense>
 
 struct imu_state {
-  Vec3C accel = {0, 0, 0};
+  dcont::Vec3C accel = {0, 0, 0};
-  QuatC rot = {0, 0, 0, 1};
+  dcont::QuatC rot = {0, 0, 0, 1};
   int64_t last_time = -1;
   Eigen::Vector3f angvel;
   Eigen::Vector3f rot_euler;

main/main.cpp

@@ -1,5 +1,7 @@
 
+#include "Eigen/Core"
 #include "driver/gpio.h"
+#include "drone.h"
 #include "drone_comms.h"
 #include "esp32-hal.h"
 #include "esp_log.h"
@@ -10,6 +12,7 @@
 #include <Arduino.h>
 #include <cstddef>
 #include <cstdint>
+#include <optional>
 
 #include "env_sens.h"
 #include "gps.h"
@@ -30,8 +33,7 @@ extern "C" void app_main(void) {
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
-  static imu_state imu_state;
+  setup_imu();
-  auto _ = setup_imu(&imu_state);
 
   xTaskCreatePinnedToCore(radio_task,   // Function name
                           "radio_rxtx", // Name for debugging
@@ -42,6 +44,15 @@ extern "C" void app_main(void) {
                           0             // Core ID
   );
 
+  xTaskCreatePinnedToCore(drone_controller_task,   // Function name
+                          "drone_controller_task", // Name for debugging
+                          1024 * 32,               // Stack size in bytes
+                          NULL,                    // Parameters
+                          20,   // Priority (higher = more urgent)
+                          NULL, // Task handle
+                          1     // Core ID
+  );
+
   xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
 
   xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
@@ -52,25 +63,28 @@ extern "C" void app_main(void) {
   Eigen::Vector3f local_vel = {0, 0, 0};
   bool nav_data_ready = false;
   while (true) {
-    if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
+    while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
       handle_packet(&packet_data[0]);
     }
 
+    std::optional<Eigen::Vector3f> coords;
+    float lat, lon, alt;
     if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
       if (gps->gps_avaliable()) {
-        ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
+        coords = gps->get_coordinates();
-                 gps->gps->latitudeDegrees, gps->gps->longitudeDegrees,
+        lat = gps->gps->latitudeDegrees;
-                 gps->gps->altitude);
+        lon = gps->gps->longitudeDegrees;
-
+        alt = gps->gps->altitude;
-        auto D_pos_cond = gps->get_coordinates();
-        if (D_pos_cond.has_value()) {
-          auto D_pos = D_pos_cond.value();
-
-          ESP_LOGI(TAG, "    -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
-                   D_pos[2]);
-        }
       }
       xSemaphoreGive(gps_mutex);
+
+      if (coords.has_value()) {
+        auto D_pos = coords.value();
+
+        ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", lat, lon, alt);
+        ESP_LOGI(TAG, "    -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
+                 D_pos[2]);
+      }
     }
     env_sens::dbg_sens();
 

main/nav.cpp

@@ -1 +0,0 @@
-#include "nav.h"