working nav kinda

main/drone.cpp

@@ -148,7 +148,7 @@ void drone_controller_task(void *params) {
   }
 }
 
-const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC,
+const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_NC, GPIO_NUM_NC,
                                   GPIO_NUM_NC};
 
 void motor_throttles_task(void *params) {

main/drone.h

@@ -1,9 +1,5 @@
 #pragma once
 
-#include "Eigen/Core"
-#include "drone_comms.h"
-#include "drone_controller.h"
-
 #ifdef PS
 #undef PS
 #endif
@@ -14,16 +10,19 @@
 
 #include <Eigen/Dense>
 
+#include "drone_comms.h"
+#include "drone_controller.h"
+
 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;
+inline dcont::ModeInput current_input_mode = dcont::ModeInput::Acro;
 
 void drone_controller_task(void *params);
 
 void motor_throttles_task(void *params);
 
-inline float motor_throttles[4];
+inline float motor_throttles[4] = {0.5, 0.5, 0.5, 0.5};
 inline bool killswitch_active;

main/env_sens.cpp

@@ -1,21 +1,13 @@
 #include "env_sens.h"
+
+#include "sens_fus.h"
+
 #include "esp_log.h"
 #include <Adafruit_BME280.h>
 #include <SPI.h>
 #include <Wire.h>
 
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include <Eigen/Dense>
-
 #include "freertos/idf_additions.h"
-#include "sens_fus.h"
 
 #define SEALEVELPRESSURE_HPA (1030)
 
@@ -34,6 +26,8 @@ void setup() {
   if (!bme.begin()) {
 
     ESP_LOGE(TAG, "Couldn't find a valid sensor");
+
+    ESP.restart();
     return;
   }
   ESP_LOGI(TAG, "BARO SETUP COMPLETE.");

main/env_sens.h

@@ -1,5 +1,15 @@
 #pragma once
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 #include "freertos/idf_additions.h"
 namespace env_sens {
 

main/gps.cpp

@@ -1,7 +1,9 @@
 #include "gps.h"
-#include "Eigen/Core"
+
+#include "esp32-hal.h"
 #include "esp_log.h"
 #include "sens_fus.h"
+#include <cstdint>
 
 static const char *TAG = "GPS_TASK";
 
@@ -12,6 +14,7 @@ void gps_poll_task(void *_) {
 
   ESP_LOGI(TAG, "GPS TASK INIT.");
 
+  uint64_t last_time = millis();
   while (true) {
     bool has_new_data = false;
     bool available = false;
@@ -34,19 +37,21 @@ void gps_poll_task(void *_) {
     }
 
     if (has_new_data && available && sens_fus_mutex) {
+
+      uint64_t current_time = millis();
       if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
         sens_fus.measure_gps(
-            1.0f, local_coords.value(),
+            (current_time - last_time) / 1000.0f, local_coords.value(),
             Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
 
-        ESP_LOGI(TAG, "applied gps measure to sens_fus");
         xSemaphoreGive(sens_fus_mutex);
+        last_time = current_time;
       } else {
 
         ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
       }
     }
 
-    vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling
+    vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling
   }
 }

main/gps.h

@@ -56,7 +56,12 @@ struct GPS {
   std::optional<Eigen::Vector3f>
   waypoint_to_xyz(float latitude, float longitude, float height) {
     if (this->origin_lat == INFINITY || this->origin_long == INFINITY) {
-      return std::nullopt;
+      if (this->gps_avaliable()) {
+        this->origin_lat = this->gps->latitudeDegrees;
+        this->origin_long = this->gps->longitudeDegrees;
+      } else {
+        return std::nullopt;
+      }
     }
 
     float dLat = (latitude - this->origin_lat) * TO_RAD;
@@ -72,8 +77,7 @@ 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->longitudeDegrees == 0.0)) {
       return std::nullopt;
     }
     float latitude = this->gps->latitudeDegrees;
@@ -83,12 +87,12 @@ struct GPS {
   }
 
   void poll() {
-    // char c = this->gps->read();
+    char c = this->gps->read();
-    // if (this->gps->newNMEAreceived()) {
+    if (this->gps->newNMEAreceived()) {
-    //   char *line = this->gps->lastNMEA();
+      char *line = this->gps->lastNMEA();
-    //   ESP_LOGI("GPS", "NMEA LINE: %s", line);
+      // ESP_LOGI("GPS", "NMEA LINE: %s", line);
-    //   this->gps->parse(line);
+      this->gps->parse(line);
-    // }
+    }
   }
 };
 

main/imu.cpp

@@ -1,8 +1,5 @@
 #include "imu.h"
-#include "esp_log.h"
+#include "Esp.h"
-#include "esp_timer.h"
-#include "hal/spi_types.h"
-#include "sens_fus.h"
 
 #ifdef PS
 #undef PS
@@ -14,6 +11,11 @@
 
 #include <Eigen/Dense>
 
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "hal/spi_types.h"
+#include "sens_fus.h"
+
 #include "freertos/idf_additions.h"
 
 static const char *TAG = "IMU";
@@ -28,6 +30,7 @@ void setup_imu() {
 
   if (!imu->initialize()) {
     ESP_LOGE(TAG, "BNO08x Init failure.");
+    ESP.restart();
   }
 
   imu->dynamic_calibration_autosave_enable();
@@ -75,16 +78,16 @@ void setup_imu() {
         dcont::Vec3C accel_global =
             dcont::apply_rot(&local_state->accel, &local_state->rot);
 
-        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) {
+        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
 
-          ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x,
+          // ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x,
-                   accel_global.y, accel_global.z, dt);
+          //          accel_global.y, accel_global.z, dt);
 
           sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
                                                accel_global.z));
 
-          ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(),
+          // ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(),
-                   sens_fus.velocity.z(), sens_fus.velocity.y());
+          // sens_fus.velocity.z(), sens_fus.velocity.y());
           xSemaphoreGive(sens_fus_mutex);
         } else {
           ESP_LOGE(TAG, "Failed to get sens_fus mutex.");

main/main.cpp

@@ -1,14 +1,3 @@
-
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include "Eigen/Core"
-
 #include "driver/gpio.h"
 #include "drone.h"
 #include "drone_comms.h"
@@ -17,8 +6,7 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
-#include "freertos/task.h"
+#include "freertos/task.h" #include < cstddef>
-#include <cstddef>
 #include <cstdint>
 #include <optional>
 
@@ -35,6 +23,7 @@ static const char *TAG = "MAIN";
 extern "C" void app_main(void) {
 
   sens_fus_mutex = xSemaphoreCreateMutex();
+  configASSERT(sens_fus_mutex);
   nav_mutex = xSemaphoreCreateMutex();
 
   initArduino();
@@ -54,24 +43,24 @@ extern "C" void app_main(void) {
   //
   xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
   //
-  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
-                          1     // Core ID
-  );
-
-  // xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
-  //                         "motor_throttles_task", // Name for debugging
-  //                         1024 * 4,               // Stack size in bytes
-  //                         NULL,                   // Parameters
-  //                         30,   // Priority (higher = more urgent)
   //                         NULL, // Task handle
   //                         1     // Core ID
   // );
 
+  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
+                          "motor_throttles_task", // Name for debugging
+                          1024 * 4,               // Stack size in bytes
+                          NULL,                   // Parameters
+                          30,   // Priority (higher = more urgent)
+                          NULL, // Task handle
+                          1     // Core ID
+  );
+
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
 
   setup_imu();
@@ -92,19 +81,31 @@ extern "C" void app_main(void) {
 
       std::optional<Eigen::Vector3f> coords;
       float lat, lon, alt;
-      if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
+      bool gps_values = false;
+      bool fix = false;
+      uint8_t sat_count = 0;
+      if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
         if (gps->gps_avaliable()) {
           coords = gps->get_coordinates();
           lat = gps->gps->latitudeDegrees;
           lon = gps->gps->longitudeDegrees;
           alt = gps->gps->altitude;
+          gps_values = true;
         }
+        sat_count = gps->gps->satellites;
+        fix = gps->gps->fix;
         xSemaphoreGive(gps_mutex);
 
+        if (gps_values) {
+
+          ESP_LOGI(TAG,
+                   "loc -> lat: %f, long: %f, height: %f, sat_c: %d, fix: %b",
+                   lat, lon, alt, sat_count, fix);
+        }
+
         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]);
         }
@@ -125,6 +126,13 @@ extern "C" void app_main(void) {
         ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
                  local_vel[2]);
       }
+
+      ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0],
+               motor_throttles[1], motor_throttles[2], motor_throttles[3]);
+
+      ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)",
+               current_controller_input.lx, current_controller_input.ly,
+               current_controller_input.rx, current_controller_input.ry);
     }
 
     vTaskDelay(pdMS_TO_TICKS(10));

main/nav.h

@@ -1,15 +1,4 @@
 #pragma once
-
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include <Eigen/Dense>
-
 #include "freertos/idf_additions.h"
 #include "gps.h"
 #include <cmath>

main/packet_handler.cpp

@@ -1,13 +1,4 @@
-
+#include "packet_handler.h"
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include "Eigen/Core"
 
 #include "drone.h"
 #include "drone_comms.h"
@@ -16,7 +7,6 @@
 #include "freertos/idf_additions.h"
 #include "gps.h"
 #include "nav.h"
-#include "packet_handler.h"
 #include "portmacro.h"
 #include "radio.h"
 #include "sens_fus.h"

main/packet_handler.h

@@ -2,17 +2,8 @@
 
 #include <cstdint>
 
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
 #include "drone_comms.h"
 #include "freertos/idf_additions.h"
-#include <Eigen/Dense>
 
 void handle_packet(uint8_t *packet_addr);
 

main/radio.cpp

@@ -1,15 +1,5 @@
 #include "radio.h"
 
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include <Eigen/Dense>
-
 #include "Esp.h"
 #include "esp32-hal-gpio.h"
 #include "esp_log.h"
@@ -68,6 +58,7 @@ void radio_task(void *pvParameters) {
     radio.setHighPower(true);
     radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
     ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
+
   } else {
     ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
     ESP.restart();

main/sens_fus.h

@@ -34,15 +34,15 @@ struct sens_fus_compl {
    * so at t=tau, were 63% of the way there
    * at t=3*tau, were 95% of the way there
    */
-  Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, 10.0f};
+  Eigen::Vector3f tau_gps_pos = {20.0f, 20.0f, INFINITY};
-  Eigen::Vector3f tau_gps_vel = {5.0f, 5.0f, INFINITY};
+  Eigen::Vector3f tau_gps_vel = {40.0f, 40.0f, INFINITY};
 
-  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f};
-  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f};
 
-  float tau_yaw = 10.0f; // Yaw remains a scalar
+  float tau_yaw = 20.0f; // Yaw remains a scalar
 
-  Eigen::Matrix3f yaw_rotation_matrix;
+  Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
 
   void update_yaw_matrix() {
     yaw_rotation_matrix =
@@ -81,12 +81,13 @@ struct sens_fus_compl {
       this->yaw_offset =
           std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
       this->update_yaw_matrix();
-    }
 
-    // res = (1 - alpha) * state + alpha * measurement
+      this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
-    this->velocity =
+                           this->velocity.array() +
-        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
+                       alpha_vel.array() * gps_vel.array();
-        alpha_vel.array() * gps_vel.array();
+    } else if (this->velocity.norm() > 1.0) {
+      this->velocity *= 1 - (0.90 * dt);
+    }
   }
 
   void measure_baro(float dt, Eigen::Vector3f baro_pos,