diff --git a/main/drone.cpp b/main/drone.cpp
index a905ca2..2903a72 100644
--- a/main/drone.cpp
+++ b/main/drone.cpp
@@ -50,16 +50,16 @@ dcont::ControllerConfig default_config() {
 
   // Rotation Loop (Rotation/Accel -> Angular Rate)
   config.stack.rotation_pid = {
-      .kp = {30.0f, 40.0f, 1.0f},
-      .ki = {2.0f, 2.0f, 0.2f},
+      .kp = {0.05f, 0.05f, 1.0f},
+      .ki = {0.0f, 0.0f, 0.2f},
       .kd = {0.0f, 0.0f, 0.0f},
-      .integral_cap = {10.0f, 10.0f, 40.0f},
+      .integral_cap = {10.0f, 1.0f, 2.0f},
       .frequency = 200.0f,
   };
 
   // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
   config.stack.rate_pid = {
-      .kp = {0.0f, 0.0f, 2.0f},
+      .kp = {0.05f, 0.05f, 2.0f},
       .ki = {0.00f, 0.00f, 0.0f},
       .kd = {0.00f, 0.00f, 0.0f},
       .integral_cap = {1.0f, 1.0f, 1.0f},
diff --git a/main/drone.h b/main/drone.h
index bf137f0..0f1ba62 100644
--- a/main/drone.h
+++ b/main/drone.h
@@ -153,32 +153,29 @@ 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: {
       if (controller_input_semaphore &&
           xSemaphoreTake(controller_input_semaphore, 10)) {
-        cont_input = current_controller_input;
+        c = current_controller_input;
 
         xSemaphoreGive(controller_input_semaphore);
 
-        auto inp =
-            dcont::Input{.joystick = {.throttle_input = 0.5,
-                                      .roll_input = cont_input.rx,
-                                      .yaw_input = cont_input.lx,
-                                      .pitch_input = cont_input.ry},
-                         .acceleration = {0.0, 0.0, 0.0},
-                         .rotation = {0.0, 0.0, 0.0},
-                         .velocity = {2 * cont_input.rx, 2 * cont_input.ry,
-                                      2 * cont_input.ly},
-                         .position = {0.0, 0.0, 0.0},
-                         .mode = dcont::ModeInput::Rotation};
+        auto inp = 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 = {-c.ry, c.rx, c.lx},
+                                .velocity = {0.0, 0.0, 0.0},
+                                .position = {0.0, 0.0, 0.0},
+                                .mode = dcont::ModeInput::Rotation};
 
         dcont::set_input(drone_controller, inp);
 
-        // ESP_LOGI("TEST", "(%f,%f), (%f,%f)", cont_input.lx, cont_input.ly,
-        //          cont_input.rx, cont_input.ry);
+        // ESP_LOGI("TEST", "(%f,%f), (%f,%f)", c.lx, c.ly, c.rx, c.ry);
 
       } else {
         ESP_LOGE("TAG", "ERRR");
diff --git a/main/main.cpp b/main/main.cpp
index 15ea7e2..672f4f5 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -99,7 +99,7 @@ extern "C" void app_main(void) {
   );
   servo_init();
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
-  ESP_LOGI("MAIN", "FLASHED");
+  ESP_LOGI("MAIN", "FLASHED2");
 
   Eigen::Vector3f local_pos = {0, 0, 0};
   Eigen::Vector3f local_vel = {0, 0, 0};
@@ -112,24 +112,12 @@ extern "C" void app_main(void) {
   bool released = false;
   bool active = false;
 
-  while (true) {
-    vTaskDelay(pdMS_TO_TICKS(2000));
-    current_controller_input.lx = 0;
-    vTaskDelay(pdMS_TO_TICKS(2000));
-    current_controller_input.lx = M_PI / 2.0;
-  }
-
   while (true) {
 
     if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
       send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
-      // ESP_LOGI("RADIO_TX_INVOK", "INFO DRONE POSITION");
       last_position_broadcast_time = millis();
     }
-    // auto vel = sens_fus.velocity;
-    // ESP_LOGI("GPSvsIMU", "gps_vel: %f, imu_vel: %f",
-    //          gps->velocity().value().norm(),
-    //          Eigen::Vector3f(vel.x(), vel.y(), 0.0).norm());
 
     if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) {
       send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS);
diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp
index afa4282..21cd02e 100644
--- a/main/packet_handler.cpp
+++ b/main/packet_handler.cpp
@@ -65,7 +65,7 @@ void handle_packet(uint8_t *packet_addr) {
         (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
 
     if (xSemaphoreTake(controller_input_semaphore, 10)) {
-      // current_controller_input = *packet;
+      current_controller_input = *packet;
       time_last_controller = millis();
       xSemaphoreGive(controller_input_semaphore);
     }