logger task

analyze.py

@@ -0,0 +1,81 @@
+import io
+import re
+
+import matplotlib.pyplot as plt
+import pandas as pd
+from pandas.io.parsers.readers import csv
+
+# 1. Define your log file path
+file_path = "log.log"
+
+# 2. Extract and parse data
+cleaned_data = []
+
+import re
+
+
+def clean_and_extract_logs(log_lines):
+    """
+    Cleans log lines of ANSI colors, filters by DCONT_DBG,
+    and ensures lines contain only numbers, commas, and dots.
+    """
+    ansi_escape = re.compile(r"\x1b\[[0-9;]*m")
+    # This pattern matches strings containing only digits, commas, and dots
+    # ^ = start of string, $ = end of string, + = at least one character
+    allowed_pattern = re.compile(r"^[0-9,.\-]+$")
+
+    csv_data = []
+
+    for line in log_lines:
+        clean_line = ansi_escape.sub("", line)
+
+        if "DCONT_DBG:" in clean_line:
+            parts = clean_line.split("DCONT_DBG:")
+            if len(parts) > 1:
+                row_values = parts[1].strip()
+
+                # Only add to csv_data if it matches the strict pattern
+                if allowed_pattern.match(row_values):
+                    csv_data.append(row_values + "\n")
+
+    return csv_data
+
+
+with open(file_path, "r") as f:
+    lines = f.readlines()
+    clean = clean_and_extract_logs(lines)
+    with open("out.csv", "w") as f:
+        f.write(
+            "rot_x,rot_y,rot_z,angvel_x,angvel_y,angvel_z,target_mot_x,target_mot_y,target_mot_z,target_angular_vel_x,target_angular_vel_y,target_angular_vel_z,target_rot_x,target_rot_y,target_rot_z\n"
+        )
+        f.writelines(clean)
+
+df = pd.read_csv("out.csv")
+
+# Define groups based on your header
+groups = {
+    "Rotation": ["rot_x", "rot_y", "rot_z"],
+    "Angular Velocity": ["angvel_x", "angvel_y", "angvel_z"],
+    "Target Motors": ["target_mot_x", "target_mot_y", "target_mot_z"],
+    "Target Angular Velocity": [
+        "target_angular_vel_x",
+        "target_angular_vel_y",
+        "target_angular_vel_z",
+    ],
+    "Target Rotation": ["target_rot_x", "target_rot_y", "target_rot_z"],
+}
+
+# Create subplots
+fig, axes = plt.subplots(len(groups), 1, figsize=(12, 18), sharex=True)
+
+# Plotting each group
+for ax, (title, columns) in zip(axes, groups.items()):
+    for col in columns:
+        ax.plot(df.index, df[col], label=col)
+    ax.set_title(title)
+    ax.legend(loc="upper right")
+    ax.grid(True, linestyle="--", alpha=0.7)
+
+plt.xlabel("Time Index / Sample")
+plt.tight_layout()
+plt.show()

flake.nix

@@ -31,7 +31,7 @@
               openssl
               stdenv.cc.cc.lib
               libclang
-              (pkgs.python3.withPackages
+              (pkgs.python312.withPackages
                 (python-pkgs: with python-pkgs; [ pandas matplotlib ]))
             ] ++ (if system == "aarch64-darwin" then [ ] else [ gdb ]);
           shellHook = ''

logs/analyze.py

@@ -0,0 +1,82 @@
+import io
+import re
+
+import matplotlib.pyplot as plt
+import pandas as pd
+from pandas.io.parsers.readers import csv
+
+# 1. Define your log file path
+file_path = "log.log"
+
+# 2. Extract and parse data
+cleaned_data = []
+
+import re
+
+
+def clean_and_extract_logs(log_lines):
+    """
+    Cleans log lines of ANSI colors, filters by DCONT_DBG,
+    and ensures lines contain only numbers, commas, and dots.
+    """
+    ansi_escape = re.compile(r"\x1b\[[0-9;]*m")
+    # This pattern matches strings containing only digits, commas, and dots
+    # ^ = start of string, $ = end of string, + = at least one character
+    allowed_pattern = re.compile(r"^[0-9,.\-]+$")
+
+    csv_data = []
+
+    for line in log_lines:
+        clean_line = ansi_escape.sub("", line)
+
+        if "DCONT_DBG:" in clean_line:
+            parts = clean_line.split("DCONT_DBG:")
+            if len(parts) > 1:
+                row_values = parts[1].strip()
+
+                # Only add to csv_data if it matches the strict pattern
+                if allowed_pattern.match(row_values):
+                    csv_data.append(row_values + "\n")
+
+    return csv_data
+
+
+with open(file_path, "r") as f:
+    lines = f.readlines()
+    clean = clean_and_extract_logs(lines)
+    with open("out.csv", "w") as f:
+        f.write(
+            "millis,rot_x,rot_y,rot_z,angvel_x,angvel_y,angvel_z,target_mot_x,target_mot_y,target_mot_z,target_angular_vel_x,target_angular_vel_y,target_angular_vel_z,target_rot_x,target_rot_y,target_rot_z\n"
+        )
+        f.writelines(clean)
+
+df = pd.read_csv("out.csv")
+
+# Define groups based on your header
+groups = {
+    "Rotation": ["rot_x", "rot_y", "rot_z"],
+    "Angular Velocity": ["angvel_x", "angvel_y", "angvel_z"],
+    "Target Motors": ["target_mot_x", "target_mot_y", "target_mot_z"],
+    "Target Angular Velocity": [
+        "target_angular_vel_x",
+        "target_angular_vel_y",
+        "target_angular_vel_z",
+    ],
+    "Target Rotation": ["target_rot_x", "target_rot_y", "target_rot_z"],
+}
+
+# Create subplots
+fig, axes = plt.subplots(len(groups), 1, figsize=(12, 18), sharex=True)
+
+# Plotting each group
+for ax, (title, columns) in zip(axes, groups.items()):
+    for col in columns:
+        ax.plot(df.index, df[col], label=col)
+        # ax.plot(df["millis"], df[col], label=col)
+    ax.set_title(title)
+    ax.legend(loc="upper right")
+    ax.grid(True, linestyle="--", alpha=0.7)
+
+plt.xlabel("Time Index / Sample")
+plt.tight_layout()
+plt.show()

main/drone.cpp

@@ -3,9 +3,16 @@
 #include "DShotRMT.h"
 #include "Eigen/Core"
 #include "Eigen/Geometry"
+
 #include "driver/rmt_tx.h"
 #include "drone_comms.h"
 #include "drone_controller.h"
+#include "esp_timer.h"
+#include "imu.h"
+#include "logger.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "sens_fus.h"
 
 #include "dshot_definitions.h"
 #include "esp32-hal.h"
@@ -15,10 +22,6 @@
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
 #include "freertos/task.h"
-#include "imu.h"
-#include "nav.h"
-#include "packet_handler.h"
-#include "sens_fus.h"
 #include "soc/gpio_num.h"
 #include <algorithm>
 #include <cstdint>
@@ -44,24 +47,24 @@ dcont::ControllerConfig default_config() {
 
   // Velocity Loop (Velocity -> Acceleration/Rotation)
   config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
-                             .ki = {0.01f, 0.01f, 0.01f},
+                             .ki = {0.00f, 0.00f, 0.00f},
                              .kd = {0.0f, 0.0f, 0.0f},
                              .frequency = 50.0f};
 
   // Rotation Loop (Rotation/Accel -> Angular Rate)
   config.stack.rotation_pid = {
-      .kp = {1.0f, 10.0f, 2.0f},
+      .kp = {30.0f, 40.0f, 2.0f},
-      .ki = {0.01f, 1.0f, 0.02f},
+      .ki = {2.0f, 2.0f, 0.02f},
-      .kd = {0.1f, 0.0f, 0.0f},
+      .kd = {0.0f, 0.0f, 0.0f},
-      .integral_cap = {4.0f, 4.0f, 4.0f},
+      .integral_cap = {10.0f, 10.0f, 4.0f},
       .frequency = 200.0f,
   };
 
   // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
   config.stack.rate_pid = {
-      .kp = {0.0f, 0.5f, 0.0f},
+      .kp = {0.066f, 0.066f, 0.66f},
       .ki = {0.00f, 0.00f, 0.0f},
-      .kd = {0.000f, 0.000f, 0.0f},
+      .kd = {0.01f, 0.01f, 0.0f},
       .integral_cap = {1.0f, 1.0f, 1.0f},
       .frequency = 500.0f,
   };
@@ -92,23 +95,27 @@ dcont::ControllerConfig default_config() {
   return config;
 }
 
-constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
+constexpr uint64_t wait_micro_sec = 1000000.0 / CONTROLLER_TASK_FREQUENCY;
 
 void drone_controller_task(void *params) {
-  drone_cont = new drone_cont_state;
+    drone_cont = new drone_cont_state;
-  drone_cont->init();
+    drone_cont->init();
 
-  while (true) {
+    const TickType_t xFrequency = pdMS_TO_TICKS(1); 
-    drone_cont->update();
+    TickType_t xLastWakeTime = xTaskGetTickCount();
-    // char *csv_str = (char
+    int i = 0;
-    // *)dcont::debug_stacked(drone_cont->drone_controller,
-    //                                              drone_cont->last_input);
-    //
-    // ESP_LOGI("DCONT_DBG", "START%sEND", csv_str);
-    // free(csv_str);
 
-    vTaskDelay(pdMS_TO_TICKS(1));
+    while (true) {
-  }
+        i++;
+
+        if (i > 20) {
+            char *csv_str = (char *)dcont::debug_stacked(
+                drone_cont->drone_controller, drone_cont->last_input, xTaskGetTickCount());
+            xQueueSend(logQueue, csv_str, 0);
+            i = 0;
+        }
+        xTaskDelayUntil(&xLastWakeTime, xFrequency);
+    }
 }
 
 const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
@@ -138,7 +145,7 @@ void motor_throttles_task(void *params) {
   while (true) {
     for (int i = 0; i < 4; i++) {
       float throttle =
-          std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.4f;
+          std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.6f;
       if (atomic_load(&killswitch_active)) {
         throttle = 0.0;
       }
@@ -148,7 +155,7 @@ void motor_throttles_task(void *params) {
     i++;
     if (i >= 50) {
 
-      ESP_LOGI("MOTORSSS", "throttles (%f,%f,%f,%f)", motor_throttles[0],
+      ESP_LOGI("MOTORS", "throttles (%f,%f,%f,%f)", motor_throttles[0],
                motor_throttles[1], motor_throttles[2], motor_throttles[3]);
       i = 0;
     }

main/drone.h

@@ -166,15 +166,19 @@ struct drone_cont_state {
 
         xSemaphoreGive(controller_input_semaphore);
 
-        auto inp = dcont::Input{.joystick = {.throttle_input = 0.5,
+        auto inp =
-                                             .roll_input = cont_input.rx,
+            dcont::Input{.joystick = {.throttle_input = 0.5,
-                                             .yaw_input = cont_input.lx,
+                                      .roll_input = cont_input.rx,
-                                             .pitch_input = cont_input.ry},
+                                      .yaw_input = cont_input.lx,
-                                .acceleration = {0.0, 0.0, 0.0},
+                                      .pitch_input = cont_input.ry},
-                                .rotation = {0.0, 0.0, 0.0},
+                         .acceleration = {0.0, 0.0, 0.0},
-                                .velocity = {0.0, 0.0, 0.0},
+                         .rotation = {0.0, 0.0, 0.0},
-                                .position = {0.0, 0.0, 0.0},
+                         .velocity = {8 * cont_input.rx, 8 * cont_input.ry,
-                                .mode = dcont::ModeInput::Rotation};
+                                      8 * cont_input.ly},
+                         .position = {0.0, 0.0, 0.0},
+                         .mode = dcont::ModeInput::Velocity};
+        // ESP_LOGI("TEST", "(%f,%f), (%f,%f)", cont_input.lx, cont_input.ly,
+        //          cont_input.rx, cont_input.ry);
 
         dcont::set_input(drone_controller, inp);
 

main/logger.cpp

@@ -0,0 +1,19 @@
+#include "logger.h"
+
+#include "esp_log.h"
+#include "esp_log_timestamp.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
+
+void init_logging_queue() { logQueue = xQueueCreate(10, sizeof(char *)); }
+
+void logger_task(void *pvParameters) {
+  char *string_to_log = nullptr;
+  while (true) {
+    if (xQueueReceive(logQueue, string_to_log, portMAX_DELAY) == pdTRUE) {
+
+      ESP_LOGI("LOGGER", "DCONT_DBG: %s", string_to_log);
+      free(string_to_log);
+    }
+  }
+}

main/logger.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include "esp_log.h"
+#include "esp_log_timestamp.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
+
+inline QueueHandle_t logQueue = nullptr;
+
+void init_logging_queue();
+
+void logger_task(void *pvParameters);

main/main.cpp

@@ -17,8 +17,10 @@
 #include "env_sens.h"
 #include "gps.h"
 #include "imu.h"
+#include "logger.h"
 #include "nav.h"
 #include "packet_handler.h"
+#include "portmacro.h"
 #include "radio.h"
 #include "sens_fus.h"
 #include "servo.h"
@@ -37,6 +39,17 @@ extern "C" void app_main(void) {
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
+  init_logging_queue();
+
+  xTaskCreatePinnedToCore(logger_task,   // Function name
+                          "logger_task", // Name for debugging
+                          2048,          // Stack size in bytes
+                          NULL,          // Parameters
+                          2,             // Priority (higher = more urgent)
+                          NULL,          // Task handle
+                          0              // Core ID
+  );
+
   xTaskCreatePinnedToCore(radio_task,   // Function name
                           "radio_rxtx", // Name for debugging
                           4096,         // Stack size in bytes
@@ -51,24 +64,18 @@ extern "C" void app_main(void) {
 
   xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
 
-  // vTaskDelay(5000);
-  // for (int i = 0; i < 4; i++) {
-  //   motor_throttles[i] = 10.0;
-  //   vTaskDelay(2000);
-  //   motor_throttles[i] = 0.0;
-  // }
-
   xTaskCreate(
       [](void *pvParameters) {
         while (true) {
-          while (packet_rx_queue &&
+          while (
-                 xQueueReceive(packet_rx_queue, &packet_data[0], 20)) {
+              packet_rx_queue &&
+              xQueueReceive(packet_rx_queue, &packet_data[0], portMAX_DELAY)) {
             handle_packet(&packet_data[0]);
           }
-          vTaskDelay(10);
+          vTaskDelay(1);
         }
       },
-      "lambda_recv_task", 8192, NULL, 5, NULL);
+      "task_recv_task", 8192, NULL, 5, NULL);
 
   setup_imu();