Merge pull request #47 from UBC-Rocket/embedded-software/controls-pid-refactor/stage

Embedded software/controls pid refactor/stage

Author: Shengw3n

.vscode/settings.json

@@ -0,0 +1,4 @@
+{
+    "cmake.sourceDirectory": "/home/j1m1/Personal/Dev/Projects/UBCRocket/thrust_vectoring_consolidated/embedded-software/ground-station",
+    "C_Cpp.errorSquiggles": "disabled"
+}

embedded-software/firmware/libs/controls/CMakeLists.txt

@@ -16,6 +16,7 @@ target_include_directories(controls
 target_link_libraries(controls
     PUBLIC
         state_estimation
+        utilities
     PRIVATE
         m
 )

embedded-software/firmware/libs/controls/include/controls/details/flight_controller_internal.h

@@ -0,0 +1,39 @@
+#ifndef FLIGHT_CONTROLLER_INTERNAL_H
+#define FLIGHT_CONTROLLER_INTERNAL_H
+
+#include "controls/flight_controller.h"
+
+void compute_axis_angle_err(const quaternion_t *q_ref,
+                            const quaternion_t *q_meas,
+                            float phi[3]);
+
+void update_torque_pid(const float phi[3],
+                       const float dt_s,
+                       float torque_cmd[3]);
+
+void decompose_torque(const float torque_cmd[3],
+                      const float thrust_dir[3],
+                      float torque_gimbal[3],
+                      float *torque_thrust_mag);
+
+void compute_thrust_dir(const flight_controller_gimbal_config_t *gcfg,
+                        const float torque_cmd[3],
+                        const float thrust_dir[3],
+                        const float thrust_mag,
+                        float new_thrust_dir[3]);
+
+void compute_gimbal_angles(const flight_controller_gimbal_config_t *gcfg,
+                           const float thrust_dir[3],
+                           float *theta_x_cmd,
+                           float *theta_y_cmd);
+
+void update_thrust_pid(const flight_controller_thrust_config_t *tcfg,
+                       const float z_ref,
+                       const float vz_ref,
+                       const float z_meas,
+                       const float vz_meas,
+                       const float dt_s,
+                       float *T_cmd);
+
+
+#endif /* FLIGHT_CONTROLLER_INTERNAL_H */ 

embedded-software/firmware/libs/controls/include/controls/flight_controller.h

@@ -18,13 +18,23 @@ extern "C" {
 /** Attitude gains and inertia (Section 3). */
 typedef struct {
     float Kp[3][3];  /**< Proportional gain: attitude error -> torque [N·m/rad] */
+    float Ki[3][3];
     float Kd[3][3];  /**< Derivative gain: angular rate -> torque [N·m·s/rad] */
     float I[3][3];   /**< Inertia matrix body frame [kg·m²] */
+    // float x_integral_limit;
+    // float y_integral_limit;
+    // float z_integral_limit;
+    // float x_torque_min;
+    // float x_torque_max;
+    // float y_torque_min;
+    // float y_torque_max;
+    // float z_torque_min;
+    // float z_torque_max;
 } flight_controller_attitude_config_t;
 
 /** Allocation: unit thrust direction in body frame (Section 4). */
 typedef struct {
-    float t_hat[3];  /**< Unit thrust direction (e.g. [0,0,-1] for z-up) */
+    float thrust_dir[3];  /**< Unit thrust direction (e.g. [0,0,-1] for z-up) */
 } flight_controller_allocation_config_t;
 
 /** Gimbal geometry and angle limits (Section 5). */
@@ -83,6 +93,13 @@ typedef struct {
  */
 void flight_controller_init(const flight_controller_config_t *config);
 
+// /**
+//  * @brief Set flight controller internal state. 
+//  * Call to update config (e.g. thrust gains) 
+//  * @param config Controller config (thrust gains used for z-PID).
+//  */
+// void flight_controller_set_config(const flight_controller_config_t *config);
+
 /**
  * @brief Run one step: torque → allocation → thrust PID → gimbal angles.
  * @param state  Current estimated state (from state_exchange).
@@ -97,6 +114,12 @@ void flight_controller_run(const state_t *state,
                            control_output_t *out,
                            float dt_s);
 
+/**
+ * @brief Reset flight controller internal state (e.g. z-PID integral).
+ * 
+ */
+void flight_controller_reset(void);
+
 #ifdef __cplusplus
 }
 #endif

embedded-software/firmware/libs/controls/include/controls/pid.h

@@ -114,6 +114,14 @@ float pid_compute(pid_controller_t *pid,
                   float measurement,
                   float dt);
 
+float pid_compute_pv(pid_controller_t *pid,
+    float x_ref,
+    float v_ref,
+    float x_meas,
+    float v_meas,
+    float dt
+);
+
 /**
  * @brief  Reset PID controller state
  * @param  pid: Pointer to PID controller structure