From 8bf1a711b28c9ab171cc23e3c3df4b4ef3d5df5d Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Sun, 19 Apr 2026 20:51:00 +0100
Subject: [PATCH] test rollback after

---
 main/drone.cpp          | 10 ++++++++++
 main/drone.h            | 16 ++++++++--------
 main/imu.cpp            |  9 +++++++--
 main/packet_handler.cpp | 12 +++++++++---
 4 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/main/drone.cpp b/main/drone.cpp
index 7db37ad..f70a1cd 100644
--- a/main/drone.cpp
+++ b/main/drone.cpp
@@ -68,6 +68,7 @@ dcont::ControllerConfig default_config() {
 
   float mixer[4][3] = {
       // x, y, z
+      //
 
       {-1.0, -1.0, -1.0}, // Rear Right
       {-1.0, 1.0, 1.0},   // Rear Left
@@ -122,6 +123,15 @@ void motor_throttles_task(void *params) {
     vTaskDelay(2);
   }
 
+  unsigned long armTime_sec = millis();
+  while (millis() - armTime_sec < 1000) {
+    for (int i = 0; i < 4; i++) {
+      motors[i]->sendThrottlePercent(20 * (millis() - armTime_sec) /
+                                     armTime_sec);
+    }
+    vTaskDelay(2);
+  }
+
   while (true) {
     for (int i = 0; i < 4; i++) {
       float throttle =
diff --git a/main/drone.h b/main/drone.h
index 19cfc60..e3bc60a 100644
--- a/main/drone.h
+++ b/main/drone.h
@@ -152,7 +152,7 @@ struct drone_cont_state {
 
   void update_input() {
 
-    packet_controller_input cont_input;
+    packet_controller_input c;
 
     switch (atomic_load(&this->current_input_mode)) {
     case INPUT_TYPE::ACRO: {
@@ -160,21 +160,21 @@ struct drone_cont_state {
         // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) {
         //   current_controller_input = {0, 0, 0, 0};
         // }
-        cont_input = current_controller_input;
+        c = current_controller_input;
 
         xSemaphoreGive(controller_input_semaphore);
 
         dcont::set_input(
             drone_controller,
-            dcont::Input{.joystick = {.throttle_input = cont_input.ly,
-                                      .roll_input = cont_input.rx,
-                                      .yaw_input = cont_input.lx,
-                                      .pitch_input = cont_input.ry},
+            dcont::Input{.joystick = {.throttle_input = c.ly,
+                                      .roll_input = c.rx,
+                                      .yaw_input = c.lx,
+                                      .pitch_input = c.ry},
                          .acceleration = {0.0, 0.0, 0.0},
-                         .rotation = {0.0, 0.0, 0.0},
+                         .rotation = {c.ry * 3, c.rx * 3, c.lx * 3},
                          .velocity = {0.0, 0.0, 0.0},
                          .position = {0.0, 0.0, 0.0},
-                         .mode = dcont::ModeInput::Acro});
+                         .mode = dcont::ModeInput::Rotation});
       } else {
         drone_cont_stabilize();
       }
diff --git a/main/imu.cpp b/main/imu.cpp
index 624a932..6824ce7 100644
--- a/main/imu.cpp
+++ b/main/imu.cpp
@@ -70,11 +70,15 @@ BNO08x *setup_imu() {
       local_state->rot =
           Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k);
 
+      // ESP_LOGI("ROT", "(%f, %f, %f, %f)", sens_rot.real, sens_rot.i,
+      // sens_rot.j,
+      //          sens_rot.k);
+
       // Eigen::Quaternionf q_global_yaw(
       //     Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
       // local_state->rot = q_global_yaw * local_state->rot;
 
-      local_state->rot.normalize();
+      // local_state->rot.normalize();
 
       // {.i = sens_rot.i, .j = sens_rot.j, .k = sens_rot.k, .w =
       // sens_rot.real}; local_state->rot_euler = {sens_euler.x, sens_euler.y,
@@ -89,7 +93,6 @@ BNO08x *setup_imu() {
 
       auto cal_gyro = imu->rpt.cal_gyro.get();
       local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z};
-      // ESP_LOGI("ROT", "angvel_z: %f", cal_gyro.z);
     }
 
     if (imu->rpt.linear_accelerometer.has_new_data()) {
@@ -142,6 +145,8 @@ BNO08x *setup_imu() {
       if (xSemaphoreTake(imu_state_mutex, 0)) {
         imu_state_var = *local_state;
         xSemaphoreGive(imu_state_mutex);
+      } else {
+        ESP_LOGE(TAG, "FAILED TO GET IMU STATE MUTEX");
       }
     }
   });
diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp
index 21cd02e..4506534 100644
--- a/main/packet_handler.cpp
+++ b/main/packet_handler.cpp
@@ -103,16 +103,22 @@ void send_packet_getter(PACKET_TYPE requested_type) {
 
   if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
 
-    Eigen::Vector3f local_vel = imu_state_var.rot.inverse() * sens_fus.velocity;
+    imu_state state;
+    if (xSemaphoreTake(imu_state_mutex, 20)) {
+      state = imu_state_var;
+      xSemaphoreGive(imu_state_mutex);
+    }
+    Eigen::Vector3f local_vel = state.rot.inverse() * sens_fus.velocity;
 
+    ESP_LOGE("Radio tx", "[f,%f,%f,%f]", state.rot.w(), state.rot.x(),
+             state.rot.y(), state.rot.z());
     resp_packet = create_packet_pooled(
         PACKET_TYPE::INFO_DRONE_POSITION,
         packet_info_drone_position{
             .trans = {sens_fus.position.x(), sens_fus.position.y(),
                       sens_fus.position.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()},
+            .rot = {state.rot.w(), state.rot.x(), state.rot.y(), state.rot.z()},
         });
   }