draft

pitch and yaw not working

-runs into segmentation fault after a few seconds

Makefile

@@ -134,6 +134,7 @@ src/devices/src/remote.c \
 src/devices/src/imu_task.c \
 src/devices/src/referee_system.c \
 src/devices/src/jetson_orin.c \
+src/devices/src/debugger.c \
 src/app/src/motor_task.c \
 src/app/src/chassis_task.c \
 src/app/src/gimbal_task.c \

src/app/inc/robot_tasks.h

@@ -99,6 +99,7 @@ void Robot_Tasks_Debug(void const *argument)
     portTickType xLastWakeTime;
     xLastWakeTime = xTaskGetTickCount();
     const TickType_t TimeIncrement = pdMS_TO_TICKS(DEBUG_PERIOD);
+    Debug_Task_Init();
     while (1)
     {
         Debug_Task_Loop();

src/app/src/debug_task.c

@@ -1,5 +1,5 @@
 #include "debug_task.h"
-#include "bsp_serial.h"
+#include "debugger.h"
 #include "launch_task.h"
 #include "remote.h"
 #include "gimbal_task.h"
@@ -15,41 +15,48 @@ extern Robot_State_t g_robot_state;
 extern DJI_Motor_Handle_t *g_yaw;
 extern IMU_t g_imu;
 extern Swerve_Module_t g_swerve_fl;
-extern Remote_t g_remote; 
+extern Remote_t g_remote;
 extern Launch_Target_t g_launch_target;
 extern uint64_t t;
 extern Daemon_Instance_t *g_daemon_instances[3];
 extern Daemon_Instance_t *g_remote_daemon;
 #define PRINT_RUNTIME_STATS
 #ifdef PRINT_RUNTIME_STATS
-char g_debug_buffer[1024*2] = {0};
+char g_debug_buffer[1024 * 2] = {0};
 #endif
 
-const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n";
-const char* bottom_border = "/***** End of Info *****/\r\n";
+const char *top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n";
+const char *bottom_border = "/***** End of Info *****/\r\n";
 
-#define DEBUG_ENABLED
+#define DEBUG_ENABLED 1
+
+void Debug_Task_Init(void)
+{
+}
 
 void Debug_Task_Loop(void)
 {
-#ifdef DEBUG_ENABLED
-    static uint32_t counter = 0;
-    #ifdef PRINT_RUNTIME_STATS
-    if (counter % 100 == 0) // Print every 100 cycles
-    {
-        vTaskGetRunTimeStats(g_debug_buffer);
-        DEBUG_PRINTF(&huart6, "%s", top_border);
-        DEBUG_PRINTF(&huart6, "%s", g_debug_buffer);
-        DEBUG_PRINTF(&huart6, "%s", bottom_border);
-    }
+    #ifdef DEBUG_ENABLED
+        static uint32_t counter = 0;
+        #ifdef PRINT_RUNTIME_STATS
+            if (counter % 100 == 0) // Print every 100 cycles
+            {
+                vTaskGetRunTimeStats(g_debug_buffer);
+                Debugger_Log_Data("%s", top_border);
+                Debugger_Log_Data("%s", g_debug_buffer);
+                Debugger_Log_Data("%s", bottom_border);
+            }
+        #endif
+        Debugger_Log_Data(">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD,
+        g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll);
+
+        //Debugger_Log_Data(">yaw:%f\n", g_imu.deg.yaw);
+        Debugger_Log_Data(">remote_daemon:%d\n", g_remote_daemon->counter);
+        counter++;
+        if (counter > 0xFFFFFFFF)
+        {
+            counter = 0;
+        }
     #endif
-
-    DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, 
-                g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll);
-    DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter);
-    counter++;
-    if (counter > 0xFFFFFFFF) {
-        counter = 0;
-    }
-#endif
-}
+}
+

src/app/src/robot.c

@@ -12,6 +12,7 @@
 #include "user_math.h"
 #include "referee_system.h"
 #include "buzzer.h"
+#include "debugger.h"
 
 extern DJI_Motor_Handle_t *g_yaw;
 #define SPIN_TOP_OMEGA (1.0f)
@@ -21,6 +22,8 @@ extern DJI_Motor_Handle_t *g_yaw;
 #define CONTROLLER_RAMP_COEF (0.8f)
 #define MAX_SPEED (.6f)
 
+#define USE_REFEREE_SYSTEM 0
+
 Robot_State_t g_robot_state = {0, 0};
 Key_Prev_t g_key_prev = {0};
 extern Launch_Target_t g_launch_target;
@@ -47,8 +50,15 @@ void Robot_Init()
     Launch_Task_Init();
     Remote_Init(&huart3);
     CAN_Service_Init();
-    Referee_System_Init(&huart1);
     Jetson_Orin_Init(&huart6);
+
+    #if defined(USE_REFEREE_SYSTEM) && USE_REFEREE_SYSTEM
+        Referee_System_Init(&huart1);
+    #elif defined(USE_REFEREE_SYSTEM) && !USE_REFEREE_SYSTEM
+        Debugger_Init(&huart1);
+    #else
+    #error "REFEREE SYSTEM AND DEBUGGER ARE NOT DEFINED!"
+    #endif
     //  Initialize all tasks
     Robot_Tasks_Start();
 }
@@ -131,7 +141,7 @@ void Robot_Cmd_Loop()
             {
                 if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0)
                 {
-                    g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw - g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin
+                    g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw - g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI);         // + orin
                     g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch - g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin
                 }
             }
@@ -203,9 +213,9 @@ void Robot_Cmd_Loop()
             float power_buffer = Referee_System.Power_n_Heat.Chassis_Power_Buffer / 60.0f;
             if (power_buffer < 0.8f)
             {
-                g_robot_state.chassis_x_speed *= pow(power_buffer,1);
-                g_robot_state.chassis_y_speed *= pow(power_buffer,1);
-                g_robot_state.chassis_omega *= pow(power_buffer,1);
+                g_robot_state.chassis_x_speed *= pow(power_buffer, 1);
+                g_robot_state.chassis_y_speed *= pow(power_buffer, 1);
+                g_robot_state.chassis_omega *= pow(power_buffer, 1);
             }
         }
     }

src/devices/inc/debugger.h

@@ -0,0 +1,23 @@
+#ifndef DEBUGGER_H
+#define DEBUGGER_H
+
+#include "bsp_uart.h"
+#include "bsp_daemon.h"
+#include "bsp_serial.h"
+
+#define DEBUGGER_DATA_RX_BUFER_SIZE (1)
+#define DEBUGGER_DATA_TX_BUFER_SIZE (1024)
+
+#define DEBUGGER_TIMEOUT_MS (10000)
+#define DEBUGGER_PERIOD (100)
+
+typedef struct {
+    uint8_t rx_buffer[DEBUGGER_DATA_RX_BUFER_SIZE];
+    uint8_t tx_buffer[DEBUGGER_DATA_TX_BUFER_SIZE];
+} Debugger_Data_t;
+
+extern Debugger_Data_t g_debugger_data;
+void Debugger_Init(UART_HandleTypeDef *huartx);
+void Debugger_Log_Data(const char *data, ...);
+
+#endif

src/devices/src/debugger.c

@@ -0,0 +1,44 @@
+#include "debugger.h"
+#include "stdarg.h"
+
+Debugger_Data_t g_debugger_data;
+
+UART_Instance_t *g_debugger_uart_instance_ptr;
+Daemon_Instance_t *g_debugger_daemon_instance_ptr;
+
+static uint8_t debugger_init = 0;
+
+void Debugger_Rx_Callback(UART_Instance_t *uart_instance)
+{
+    return;
+}
+
+void Debugger_Timeout_Callback()
+{
+    return;
+}
+
+void Debugger_Init(UART_HandleTypeDef *huartx)
+{
+    debugger_init = 1;
+    // register UART instance
+    g_debugger_uart_instance_ptr = UART_Register(huartx, g_debugger_data.rx_buffer, DEBUGGER_DATA_RX_BUFER_SIZE, Debugger_Rx_Callback);
+
+    // register Daemon instance
+    // timeout is defined in the header file
+    uint16_t reload_value = DEBUGGER_TIMEOUT_MS / DAEMON_PERIOD;
+    uint16_t initial_counter = reload_value;
+    g_debugger_daemon_instance_ptr = Daemon_Register(reload_value, initial_counter, Debugger_Timeout_Callback);
+    return;
+}
+
+void Debugger_Log_Data(const char *data, ...)
+{
+    if (debugger_init)
+    {
+        va_list args;
+        va_start(args, data);
+        DEBUG_PRINTF(g_debugger_uart_instance_ptr->uart_handle, data, args);
+        va_end(args);
+    }
+}

src/devices/src/dm_motor.c

@@ -76,7 +76,8 @@ void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
     uint8_t *data = motor->can_instance->tx_buffer;
     switch (motor->disable_behavior)
     {
-    case DM_MOTOR_ZERO_CURRENT:
+    case DM_MOTOR_ZERO_CURRENT: 
+    {
         uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp;
 
         pos_temp = float_to_uint(0, P_MIN, P_MAX, 16);
@@ -94,6 +95,7 @@ void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
         data[6] = ((kd_temp & 0xF) << 4) | (torq_temp >> 8);
         data[7] = torq_temp;
         break;
+    }
     case DM_MOTOR_HARDWARE_DISABLE:
         data[0] = 0xFF;
         data[1] = 0xFF;