From 36ea82a13cc7756d3c5994d877e819810e2c49bf Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Mon, 23 Mar 2026 11:01:23 +0000
Subject: [PATCH] add DShotRMT library. add gps file. working example using
 mot1

---
 .gitmodules         |  3 +++
 components/DShotRMT |  1 +
 main/CMakeLists.txt |  4 ++--
 main/gps.h          | 24 +++++++++++++++++++++
 main/imu.h          |  6 ++++--
 main/main.cpp       | 52 +++++++++++++++++++++++----------------------
 6 files changed, 61 insertions(+), 29 deletions(-)
 create mode 160000 components/DShotRMT
 create mode 100644 main/gps.h

diff --git a/.gitmodules b/.gitmodules
index 5ee78b5..65af168 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -17,3 +17,6 @@
 [submodule "components/esp32_Adafruit_Sensor"]
 	path = components/esp32_Adafruit_Sensor
 	url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_Sensor.git
+[submodule "components/DShotRMT"]
+	path = components/DShotRMT
+	url = https://github.com/derdoktor667/DShotRMT
diff --git a/components/DShotRMT b/components/DShotRMT
new file mode 160000
index 0000000..6e80e6a
--- /dev/null
+++ b/components/DShotRMT
@@ -0,0 +1 @@
+Subproject commit 6e80e6ad3c3f6abd40e5c99e75436fb920869f84
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 49ec69b..2963a14 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,5 +1,5 @@
-idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp" 
-                    INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen)
+idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp"
+                    INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT)
 
 target_compile_options(${COMPONENT_LIB} PRIVATE 
                        "-Wno-gnu-array-member-paren-init" 
diff --git a/main/gps.h b/main/gps.h
new file mode 100644
index 0000000..c7f1ba2
--- /dev/null
+++ b/main/gps.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "Wire.h"
+#include <Adafruit_GPS.h>
+
+struct GPS {
+  Adafruit_GPS gps;
+  GPS(TwoWire *theWire) {
+    gps = Adafruit_GPS(theWire);
+    gps.begin(0x10);
+    gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
+
+    gps.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
+    gps.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
+  }
+
+  void poll() {
+    this->gps.read();
+    if (this->gps.newNMEAreceived()) {
+      if (!this->gps.parse(this->gps.lastNMEA()))
+        return;
+    }
+  }
+};
diff --git a/main/imu.h b/main/imu.h
index 3955384..1c5615d 100644
--- a/main/imu.h
+++ b/main/imu.h
@@ -1,10 +1,12 @@
 #pragma once
 
+#include <cmath>
+#include <cstdint>
+
 #include "BNO08x.hpp"
 #include "BNO08xGlobalTypes.hpp"
 #include "drone_controller.h"
-#include <cmath>
-#include <cstdint>
+#include "freertos/idf_additions.h"
 
 struct imu_state {
   Vec3C accel = {0, 0, 0};
diff --git a/main/main.cpp b/main/main.cpp
index dc82f2f..f10fdff 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -1,36 +1,38 @@
-#include "esp_log.h"
-#include "esp_timer.h"
-#include <cmath>
-#include <cstdint>
-#include <stdio.h>
+#include "HardwareSerial.h"
+#include "dshot_utils.h"
 
+#include "esp32-hal.h"
+#include "esp_log.h"
+#include <cmath>
+#include <iostream>
+
+#include "DShotRMT.h"
 #include "env_sens.h"
-#include "freertos/idf_additions.h"
-#include "imu.h"
 
 static const constexpr char *TAG = "Main";
+const gpio_num_t MOTOR_PIN = GPIO_NUM_16;
+
+DShotRMT motor(MOTOR_PIN, DSHOT300, false);
 
 extern "C" void app_main(void) {
-  env_sens::setup();
+  initArduino();
+  motor.begin();
 
-  imu_state imu_state;
-  BNO08x *imu = setup_imu(&imu_state);
-  if (imu == nullptr) {
-    ESP_LOGE(TAG, "IMU setup failed.");
-    return;
+  ESP_LOGI(TAG, "Arming ESC...");
+  // Send 0 throttle for 4 seconds to arm
+  unsigned long armTime = millis();
+  while (millis() - armTime < 4000) {
+    motor.sendThrottlePercent(0);
   }
 
-  ESP_LOGE(TAG, "IMU setup sucess.");
-
-  while (1) {
-    vTaskDelay(pdMS_TO_TICKS(300));
-    if (xSemaphoreTake(imuStateMutex, pdMS_TO_TICKS(5)) == pdTRUE) {
-      ESP_LOGI(TAG, "accel - %f, %f, %f", imu_state.accel.x, imu_state.accel.y,
-               imu_state.accel.z);
-      ESP_LOGI(TAG, "; euler - %f, %f, %f\n", imu_state.euler_ang.x,
-               imu_state.euler_ang.y, imu_state.euler_ang.z);
-      xSemaphoreGive(imuStateMutex);
-    }
-    env_sens::dbg_sens();
+  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);
   }
+
+  ESP_LOGI(TAG, "Stopping motor");
+  motor.sendThrottlePercent(0);
 }