From 8c087ab5f36c3a9c55e522e2f8c38710797e8763 Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Wed, 25 Mar 2026 18:26:56 +0000
Subject: [PATCH] before gps test

---
 main/CMakeLists.txt |  4 +++-
 main/gps.h          |  4 +++-
 main/main.cpp       | 14 ++++++++++----
 main/pos.cpp        | 27 +++++++++++++++++++++++++++
 4 files changed, 43 insertions(+), 6 deletions(-)
 create mode 100644 main/pos.cpp

diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 2963a14..9dd3759 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,4 +1,6 @@
-idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp"
+file(GLOB_RECURSE SOURCES "*.cpp")
+
+idf_component_register(SRCS "${SOURCES}"
                     INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT)
 
 target_compile_options(${COMPONENT_LIB} PRIVATE 
diff --git a/main/gps.h b/main/gps.h
index c7f1ba2..966b6ff 100644
--- a/main/gps.h
+++ b/main/gps.h
@@ -1,12 +1,14 @@
 #pragma once
 
+#include "HardwareSerial.h"
 #include "Wire.h"
 #include <Adafruit_GPS.h>
 
 struct GPS {
   Adafruit_GPS gps;
   GPS(TwoWire *theWire) {
-    gps = Adafruit_GPS(theWire);
+    Serial2.begin(9600);
+    gps = Adafruit_GPS(&Serial2);
     gps.begin(0x10);
     gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
 
diff --git a/main/main.cpp b/main/main.cpp
index f10fdff..eba3a5a 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -10,27 +10,33 @@
 #include "env_sens.h"
 
 static const constexpr char *TAG = "Main";
-const gpio_num_t MOTOR_PIN = GPIO_NUM_16;
+const gpio_num_t MOTOR_PIN = GPIO_NUM_22;
 
-DShotRMT motor(MOTOR_PIN, DSHOT300, false);
+DShotRMT motor(MOTOR_PIN, DSHOT300, true);
 
 extern "C" void app_main(void) {
   initArduino();
-  motor.begin();
+  printf("beg: %d", motor.begin().result_code);
 
   ESP_LOGI(TAG, "Arming ESC...");
   // Send 0 throttle for 4 seconds to arm
   unsigned long armTime = millis();
   while (millis() - armTime < 4000) {
     motor.sendThrottlePercent(0);
+    // delay(1);
   }
+  delay(50);
 
   ESP_LOGI(TAG, "Motor Armed. Ramping up...");
   // Send 10% throttle for 5 seconds
   unsigned long runTime = millis();
   while (millis() - runTime < 5000) {
     auto res = motor.sendThrottlePercent(10);
-    // printf("%d, %d\n", res.result_code, res.erpm);
+    printf("%d, %d\n", res.result_code, res.erpm);
+    //
+    // ESP_LOGI(TAG, "current: %d",
+    // motor.getTelemetry().telemetry_data.current);
+    // delay(1);
   }
 
   ESP_LOGI(TAG, "Stopping motor");
diff --git a/main/pos.cpp b/main/pos.cpp
new file mode 100644
index 0000000..08ccd45
--- /dev/null
+++ b/main/pos.cpp
@@ -0,0 +1,27 @@
+#include <Eigen/Dense>
+
+struct nav_compl {
+  Eigen::Vector3f position = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+
+  Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.01f);
+  Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.01f);
+
+  void predict(float dt, Eigen::Vector3f accel) {
+    Eigen::Vector3f next_velocity = this->velocity + (accel * dt);
+
+    this->position += (this->velocity + next_velocity) * 0.5f * dt;
+
+    this->velocity = next_velocity;
+  }
+
+  void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
+    this->position = (Eigen::Vector3f::Ones() - gps_weight_pos)
+                         .cwiseProduct(this->position) +
+                     gps_weight_pos.cwiseProduct(gps_pos);
+
+    this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel)
+                         .cwiseProduct(this->velocity) +
+                     gps_weight_vel.cwiseProduct(gps_vel);
+  }
+};