sens fus filtering worked WITHOUT motor vibration

components/drone_comms

@@ -1 +1 @@
-Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2
+Subproject commit d58740e83368c9f2cc2b641ff729d04e7033d007

main/env_sens.cpp

@@ -20,7 +20,7 @@ Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
 
 static const constexpr char *TAG = "BARO";
 
-#define BARO_SDA GPIO_NUM_47
+#define BARO_SDA GPIO_NUM_47 // SDI
 #define BARO_SCL GPIO_NUM_48
 
 namespace env_sens {
@@ -141,7 +141,7 @@ void baro_poll_task(void *_) {
       last_time = now;
     }
 
-    // BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal
+    // BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
     vTaskDelay(pdMS_TO_TICKS(50));
   }
 }

main/gps.cpp

@@ -2,6 +2,7 @@
 
 #include "esp32-hal.h"
 #include "esp_log.h"
+#include "freertos/idf_additions.h"
 #include "sens_fus.h"
 #include <cstdint>
 
@@ -16,7 +17,6 @@ void gps_poll_task(void *_) {
 
   uint64_t last_time = millis();
   while (true) {
-    bool has_new_data = false;
     bool available = false;
     Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
     std::optional<Eigen::Vector3f> local_coords;
@@ -25,30 +25,37 @@ void gps_poll_task(void *_) {
       gps->poll();
 
       if (gps->gps_avaliable()) {
-        available = true;
         local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
         local_coords = gps->get_coordinates();
+
+        available = local_coords.has_value();
       }
       xSemaphoreGive(gps_mutex);
 
-      has_new_data = local_coords.has_value();
     } else {
       ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
     }
 
-    if (has_new_data && available && sens_fus_mutex) {
+    if (sens_fus_mutex) {
+      if (available) {
 
-      uint64_t current_time = millis();
+        uint64_t current_time = millis();
-      if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
+        if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
-        sens_fus.measure_gps(
+          sens_fus.measure_gps(
-            (current_time - last_time) / 1000.0f, local_coords.value(),
+              (current_time - last_time) / 1000.0f, local_coords.value(),
-            Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
+              Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
 
-        xSemaphoreGive(sens_fus_mutex);
+          xSemaphoreGive(sens_fus_mutex);
-        last_time = current_time;
+          last_time = current_time;
+        } else {
+
+          ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
+        }
       } else {
-
+        if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
-        ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
+          sens_fus.gps_lost();
+          xSemaphoreGive(sens_fus_mutex);
+        }
       }
     }
 

main/gps.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdint>
 #ifdef PS
 #undef PS
 #endif
@@ -14,14 +15,15 @@
 
 #include "esp_log.h"
 #include <Adafruit_GPS.h> #include <cmath>
+#include <ctime>
 #include <optional>
 
 const float TO_RAD = M_PI / 180.0f;
 const float KNOTS_TO_M_SEC = 0.5144444f;
 const float earth_radius = 6371000.0f;
 
-#define GPS_RX_PIN 18
+#define GPS_TX_PIN 18
-#define GPS_TX_PIN 17
+#define GPS_RX_PIN 17
 
 struct GPS {
   Adafruit_GPS *gps;
@@ -34,8 +36,34 @@ struct GPS {
     origin_long = INFINITY;
   }
 
+  uint64_t unix_timestamp_millis() {
+    struct tm t;
+
+    t.tm_year = (2000 + this->gps->year) - 1900;
+    t.tm_mon = this->gps->month - 1;
+    t.tm_mday = this->gps->day;
+    t.tm_hour = this->gps->hour;
+    t.tm_min = this->gps->minute;
+    t.tm_sec = this->gps->seconds;
+    t.tm_isdst = 0; // GPS is always UTC, so no Daylight Savings
+
+    // mktime converts struct tm to time_t (Unix timestamp)
+    time_t unixTime = mktime(&t);
+
+    // Calculate how many milliseconds have passed since the GPS updated its
+    // data
+    unsigned long msSinceUpdate = millis() - this->gps->lastDate;
+
+    // Return the base timestamp plus the elapsed seconds
+    return (uint64_t)unixTime * 1000 + msSinceUpdate + gps->milliseconds;
+  }
+
+  uint64_t unix_timestamp_seconds() {
+    return this->unix_timestamp_millis() / 1000;
+  }
+
   void begin() {
-    Serial2.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
+    Serial2.begin(9600, SERIAL_8N1, GPS_TX_PIN, GPS_RX_PIN);
     this->gps = new Adafruit_GPS(&Serial2);
     // this->gps->begin(9600);
     this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
@@ -95,7 +123,7 @@ struct GPS {
     }
     if (this->gps->newNMEAreceived()) {
       char *line = this->gps->lastNMEA();
-      // ESP_LOGI("GPS", "NMEA LINE: %s", line);
+      ESP_LOGI("GPS", "NMEA LINE: %s", line);
       this->gps->parse(line);
     }
   }

main/imu.cpp

@@ -22,13 +22,13 @@
 
 static const char *TAG = "IMU";
 
-#define IMU_CS GPIO_NUM_3
+#define IMU_CS GPIO_NUM_3   // Green
-#define IMU_MOSI GPIO_NUM_2 // DI
+#define IMU_MOSI GPIO_NUM_2 // DI - White
-#define IMU_RST GPIO_NUM_1
+#define IMU_RST GPIO_NUM_1  // Red
 
-#define IMU_INT GPIO_NUM_4
+#define IMU_INT GPIO_NUM_4  // Yellow
-#define IMU_MISO GPIO_NUM_5 // SDA
+#define IMU_MISO GPIO_NUM_5 // SDA - White
-#define IMU_SCLK GPIO_NUM_6 // SCL
+#define IMU_SCLK GPIO_NUM_6 // SCL - Green
 
 BNO08x *setup_imu() {
   imu_state *local_state = new imu_state;

main/main.cpp

@@ -33,17 +33,17 @@ extern "C" void app_main(void) {
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
-  xTaskCreatePinnedToCore(radio_task,   // Function name
+  // xTaskCreatePinnedToCore(radio_task,   // Function name
-                          "radio_rxtx", // Name for debugging
+  //                         "radio_rxtx", // Name for debugging
-                          4096,         // Stack size in bytes
+  //                         4096,         // Stack size in bytes
-                          NULL,         // Parameters
+  //                         NULL,         // Parameters
-                          5,            // Priority (higher = more urgent)
+  //                         5,            // Priority (higher = more urgent)
-                          NULL,         // Task handle
+  //                         NULL,         // Task handle
-                          0             // Core ID
+  //                         0             // Core ID
-  );
+  // );
-
+  //
-  xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5,
+  // xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL,
-                          NULL, 0);
+  // 5, NULL, 0);
 
   xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
 

main/packet_handler.cpp

@@ -106,35 +106,31 @@ void send_packet_getter(PACKET_TYPE requested_type) {
         packet_info_drone_position{
             .trans = {sens_fus.position.x(), sens_fus.position.y(),
                       sens_fus.position.z()},
-            .accel = {imu_state_var.lin_accel.x, imu_state_var.lin_accel.y,
+            .accel = {imu_state_var.lin_accel_global.x,
-                      imu_state_var.lin_accel.z},
+                      imu_state_var.lin_accel_global.y,
+                      imu_state_var.lin_accel_global.z},
             .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()},
-            .pressure = env_sens::get_pressure(),
+            .angvel = {imu_state_var.angvel.x(), imu_state_var.angvel.y(),
-            .temperature = env_sens::get_temperature()});
+                       imu_state_var.angvel.z()}});
   }
 
   if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
-
+    if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY)) {
-    if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY) == pdTRUE) {
-
-      // TODO: Absolute time from GPS instead of 0
       resp_packet = create_packet_pooled(
           PACKET_TYPE::INFO_DRONE_STATUS,
           packet_info_drone_status{
               .origin = {gps->origin_long, gps->origin_lat},
               .time_since_boot = millis(),
-              .unix_timestamp_millis = 0,
+              .unix_timestamp_millis = gps->unix_timestamp_millis(),
               .gps_fix = gps->gps_avaliable()});
-      ESP_LOGI("STATUS", "Status sent");
       xSemaphoreGive(gps_mutex);
     }
   }
 
   // Navigation
   if (requested_type == PACKET_TYPE::DRONE_NAV) {
-
     uint8_t active_mask, current;
     if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
       active_mask = nav_man.get_active_mask();

main/radio.cpp

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

main/sens_fus.h

@@ -27,9 +27,18 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps,
 struct sens_fus_compl {
   Eigen::Vector3f position = Eigen::Vector3f::Zero();
   Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
-  Eigen::Vector3f velocity_last_gps_measurment = Eigen::Vector3f::Zero();
   Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
-  float yaw_offset = 0.0f;
+
+  Eigen::Vector3f velocity_error = Eigen::Vector3f::Zero();
+  Eigen::Vector3f position_error = Eigen::Vector3f::Zero();
+
+  void gps_lost() {
+    this->position_error = Eigen::Vector3f::Zero();
+    this->velocity_error = Eigen::Vector3f::Zero();
+  }
+
+  Eigen::Vector3f velocity_error_mult = {40.0f, 40.0f, 0.0f};
+  Eigen::Vector3f position_error_mult = {40.0f, 40.0f, 0.0f};
 
   /*
    * Tau is the time that the filter takes to reach 1-e^(-1) of the difference
@@ -37,67 +46,48 @@ 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 = {2.0f, 2.0f, INFINITY};
+  Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, INFINITY};
-  Eigen::Vector3f tau_gps_vel = {INFINITY, INFINITY, INFINITY};
+  Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY};
 
-  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 10.0f};
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 4.0f};
-  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f};
-
-  float tau_yaw = 10.0f; // Yaw remains a scalar
 
   Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
 
-  void update_yaw_matrix() {
-    // yaw_rotation_matrix =
-    //     Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ())
-    //         .toRotationMatrix();
-  }
-
   void predict(float dt, Eigen::Vector3f accel) {
-    Eigen::Vector3f accel_rotated = yaw_rotation_matrix * accel;
+    Eigen::Vector3f accel_err_rmvd =
+        accel.array() -
+        this->velocity_error.array() * this->velocity_error_mult.array();
 
     Eigen::Vector3f next_velocity =
-        this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt;
+        this->velocity + (this->last_accel_world + accel_err_rmvd) * 0.5f * dt;
 
-    this->position += (this->velocity + next_velocity) * 0.5f * dt;
+    this->position =
+        this->position.array() +
+        (((this->velocity + next_velocity) * 0.5f).array() -
+         this->position_error.array() * this->position_error_mult.array()) *
+            dt;
 
     this->velocity = next_velocity;
-    this->last_accel_world = accel_rotated;
+    this->last_accel_world = accel_err_rmvd;
   }
 
   void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
     // alpha = dt / (tau + dt)
     Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
     Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
-    float alpha_yaw = dt / (tau_yaw + dt);
 
-    // res = (1 - alpha) * state + alpha * measurement
+    this->velocity_error = this->velocity - gps_vel;
+    this->position_error = this->position - gps_pos;
+
+    // next_state = (1 - alpha) * state + alpha * measurement
     this->position =
         (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
         alpha_pos.array() * gps_pos.array();
 
-    // 2. Yaw Correction (only if moving > 1.0 m/s)
+    this->velocity =
-    if (gps_vel.norm() > 0.2f) {
+        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
-      Eigen::Vector3f delta_v_imu =
+        alpha_vel.array() * gps_vel.array();
-          this->velocity - this->velocity_last_gps_measurment;
-      Eigen::Vector3f delta_v_gps = this->velocity - gps_vel;
-
-      float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
-
-      this->yaw_offset += yaw_delta * alpha_yaw;
-
-      this->yaw_offset =
-          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
-      this->update_yaw_matrix();
-
-      this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
-                           this->velocity.array() +
-                       alpha_vel.array() * gps_vel.array();
-    } else if (this->velocity.norm() > 0.2f) {
-      this->velocity *= 1 - (0.90 * dt);
-    }
-
-    this->velocity_last_gps_measurment = this->velocity;
   }
 
   void measure_baro(float dt, Eigen::Vector3f baro_pos,