bare minimum to fly with a controller. initial navigation development. TODO: mark waypoints as inactive and continue to next when current one is reached, drone stabilization logic.

components/drone_controller

@@ -1 +1 @@
-Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2
+Subproject commit ac127154714cf9ce781757d7203651fed214b5ff

main/drone.cpp

@@ -1,17 +1,26 @@
 #include "drone.h"
 
+#include "DShotRMT.h"
+#include "Eigen/Core"
+#include "drone_comms.h"
 #include "drone_controller.h"
 
+#include "dshot_definitions.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 "nav.h"
+#include "packet_handler.h"
 #include "sens_fus.h"
+#include "soc/gpio_num.h"
 #include <cstdint>
+#include <cstring>
+#include <optional>
 
-#define CONTROLLER_TASK_FREQUENCY 400.0;
+#define CONTROLLER_TASK_FREQUENCY 1000.0;
 
 dcont::ControllerConfig default_config() {
   dcont::ControllerConfig config;
@@ -39,7 +48,7 @@ dcont::ControllerConfig default_config() {
   config.stack.rate_pid = {{0.2f, 0.2f, 2.0f},
                            {0.05f, 0.05f, 0.05f},
                            {0.003f, 0.003f, 0.001f},
-                           600.0f};
+                           1000.0f};
 
   // 2. Set Constraints
   config.stack.max_rate = 3.14f;   // ~180 degrees/s
@@ -69,6 +78,13 @@ dcont::ControllerConfig default_config() {
 
 void setup_drone() { drone_controller = dcont::create(default_config()); }
 
+void drone_cont_stabilize() {
+  // TODO: Implement stabilization. if |angvel| > something, we should make it 0
+  // first, then if falling fast, pull rotation to the side, then when falling
+  // is stabilized, turn slowly up until pointing up. let velocity dissipate
+  // afterwards
+}
+
 void drone_controller_task(void *params) {
   setup_drone();
   uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
@@ -94,6 +110,69 @@ void drone_controller_task(void *params) {
     dcont::set_cur_pos(drone_controller, v3f_to_v3c(position_local));
     dcont::set_cur_rot(drone_controller, imu_state_local.rot);
 
+    packet_controller_input cont_input;
+    if (current_input_mode == dcont::ModeInput::Acro &&
+        xSemaphoreTake(controller_input_semaphore, 10)) {
+      cont_input = current_controller_input;
+
+      xSemaphoreGive(controller_input_semaphore);
+    }
+
+    waypoint wayp;
+    if (current_input_mode == dcont::ModeInput::Position &&
+        xSemaphoreTake(nav_mutex, 10)) {
+      wayp = nav_man.get_current_waypoint();
+
+      xSemaphoreGive(nav_mutex);
+    }
+    if (current_input_mode == dcont::ModeInput::Position &&
+        wayp.coords_in_axis == std::nullopt) {
+      drone_cont_stabilize();
+    } else {
+      auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero());
+
+      dcont::set_input(drone_controller,
+                       dcont::Input{{cont_input.ly, cont_input.lx,
+                                     cont_input.rx, cont_input.ry},
+                                    {0.0, 0.0, 0.0},
+                                    {0.0, 0.0, 0.0},
+                                    {0.0, 0.0, 0.0},
+                                    {coords.x(), coords.y(), coords.z()},
+                                    current_input_mode});
+    }
+
+    memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
+           sizeof(motor_throttles));
+
     vTaskDelay(pdMS_TO_TICKS(wait_ms));
   }
 }
+
+const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC,
+                                  GPIO_NUM_NC};
+
+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);
+  }
+
+  // ARM
+  unsigned long armTime = millis();
+  while (millis() - armTime < 4000) {
+    for (int i = 0; i < 4; i++) {
+      motors[i]->sendThrottlePercent(0);
+    }
+  }
+
+  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);
+      }
+    }
+    vTaskDelay(1);
+  }
+}

main/drone.h

@@ -22,3 +22,8 @@ inline dcont::StackedController *drone_controller = nullptr;
 inline dcont::ModeInput current_input_mode;
 
 void drone_controller_task(void *params);
+
+void motor_throttles_task(void *params);
+
+inline float motor_throttles[4];
+inline bool killswitch_active;

main/gps.h

@@ -19,7 +19,6 @@
 
 const float TO_RAD = M_PI / 180.0f;
 const float KNOTS_TO_M_SEC = 0.5144444f;
-
 const float earth_radius = 6371000.0f;
 
 struct GPS {
@@ -73,8 +72,8 @@ struct GPS {
   }
 
   std::optional<Eigen::Vector3f> get_coordinates() {
-    if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 ||
+    if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 &&
-        this->gps->longitudeDegrees != 0.0) {
+                                    this->gps->longitudeDegrees == 0.0)) {
       return std::nullopt;
     }
     float latitude = this->gps->latitudeDegrees;

main/imu.cpp

@@ -33,7 +33,7 @@ void setup_imu() {
 
   imu->rpt.rv_game.enable(2500UL);
   imu->rpt.linear_accelerometer.enable(2500UL);
-  imu->rpt.cal_gyro.enable(2500UL);
+  imu->rpt.cal_gyro.enable(1000UL);
 
   imu->register_cb([imu, local_state]() {
     bool needs_updating = false;

main/main.cpp

@@ -44,6 +44,10 @@ extern "C" void app_main(void) {
                           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(drone_controller_task,   // Function name
                           "drone_controller_task", // Name for debugging
                           1024 * 32,               // Stack size in bytes
@@ -53,20 +57,29 @@ extern "C" void app_main(void) {
                           1     // Core ID
   );
 
-  xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
+  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
-
+                          "motor_throttles_task", // Name for debugging
-  xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
+                          1024 * 4,               // Stack size in bytes
+                          NULL,                   // Parameters
+                          20,   // Priority (higher = more urgent)
+                          NULL, // Task handle
+                          1     // Core ID
+  );
 
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
 
   Eigen::Vector3f local_pos = {0, 0, 0};
   Eigen::Vector3f local_vel = {0, 0, 0};
   bool nav_data_ready = false;
+
+  uint64_t last_print_time = 0;
   while (true) {
     while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
       handle_packet(&packet_data[0]);
     }
 
+    if (millis() > last_print_time + 1000) {
+
       std::optional<Eigen::Vector3f> coords;
       float lat, lon, alt;
       if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
@@ -102,7 +115,8 @@ extern "C" void app_main(void) {
         ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
                  local_vel[2]);
       }
+    }
 
-    vTaskDelay(pdMS_TO_TICKS(1000));
+    vTaskDelay(pdMS_TO_TICKS(10));
   }
 }

main/nav.h

@@ -2,7 +2,10 @@
 
 #include "Eigen/Core"
 #include "freertos/idf_additions.h"
+#include "gps.h"
+#include <cmath>
 #include <cstdint>
+#include <optional>
 
 #ifdef PS
 #undef PS
@@ -17,8 +20,9 @@
 #define WAYPOINT_COUNT 8
 
 struct waypoint {
-  Eigen::Vector3f coords; // lon, lat, alt
+  Eigen::Vector3f coords; // lat, lon, alt
-  bool active;            // active or to be skipped
+  std::optional<Eigen::Vector3f> coords_in_axis = std::nullopt;
+  bool active = false; // active or to be skipped
 };
 
 struct drone_nav {
@@ -40,6 +44,16 @@ struct drone_nav {
     }
     return mask;
   }
+
+  waypoint get_current_waypoint() {
+    waypoint wayp = this->waypoints[this->current_waypoint];
+    if (!wayp.coords_in_axis.has_value()) {
+      auto axis =
+          gps->waypoint_to_xyz(wayp.coords[0], wayp.coords[1], wayp.coords[2]);
+      wayp.coords_in_axis = axis;
+    }
+    return wayp;
+  }
 };
 
 inline SemaphoreHandle_t nav_mutex = NULL;

main/packet_handler.cpp

@@ -1,7 +1,10 @@
 #include "packet_handler.h"
 
 #include "Eigen/Core"
+#include "drone.h"
 #include "drone_comms.h"
+#include "drone_controller.h"
+#include "esp_log.h"
 #include "freertos/idf_additions.h"
 #include "gps.h"
 #include "nav.h"
@@ -32,13 +35,38 @@ void handle_packet(uint8_t *packet_addr) {
   }
 
   // NAV SETTERS
-  if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
+  else if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
            packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
     uint8_t index = packet_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
     handle_waypoint_update(packet_addr, index);
-  }
+  } else if (packet_type == PACKET_TYPE::DRONE_NAV) {
-  if (packet_type == PACKET_TYPE::DRONE_NAV) {
     handle_nav_update(packet_addr);
+  } else if (packet_type == PACKET_TYPE::KILLSWITCH_TOGGLE) {
+    packet_killswitch_toggle *packet =
+        (packet_killswitch_toggle *)(packet_addr + sizeof(PACKET_TYPE));
+    killswitch_active = packet->killswitch_active;
+  } else if (packet_type == PACKET_TYPE::MODE_INPUT) {
+    packet_mode_input *packet =
+        (packet_mode_input *)(packet_addr + sizeof(PACKET_TYPE));
+    switch (packet->input_type) {
+    case INPUT_TYPE::ACRO:
+      current_input_mode = dcont::ModeInput::Acro;
+      break;
+    case INPUT_TYPE::LEVEL:
+    case INPUT_TYPE::STABILIZE_FALL:
+      ESP_LOGE("PACKET_HANDLER",
+               "INPUT TYPES NOT IMPLEMENTED. DEFAULTING TO POSITION (AUTONAV)");
+    case INPUT_TYPE::AUTO_NAV:
+      current_input_mode = dcont::ModeInput::Position;
+    }
+  } else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) {
+    packet_controller_input *packet =
+        (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
+
+    if (xSemaphoreTake(controller_input_semaphore, 10)) {
+      current_controller_input = *packet;
+      xSemaphoreGive(controller_input_semaphore);
+    }
   }
 }
 

main/packet_handler.h

@@ -2,4 +2,10 @@
 
 #include <cstdint>
 
+#include "drone_comms.h"
+#include "freertos/idf_additions.h"
+
 void handle_packet(uint8_t *packet_addr);
+
+inline SemaphoreHandle_t controller_input_semaphore = NULL;
+inline packet_controller_input current_controller_input;

main/radio.h

@@ -5,7 +5,5 @@
 
 inline QueueHandle_t packet_rx_queue = NULL;
 inline QueueHandle_t packet_tx_queue = NULL;
-inline SemaphoreHandle_t controller_input_semaphore = NULL;
-inline packet_controller_input current_controller_input;
 
 void radio_task(void *pvParameters);