Changes due to Steven's review

Author: J1m1-P

embedded-software/firmware/ulysses-flight-controller/Core/Inc/motor_drivers/servo_driver.h

@@ -12,8 +12,6 @@ typedef struct {
     float deg_min;
     float deg_max;
     float deg_bias;
-
-    bool reversed;
 } servo_config_t;
 
 typedef struct {
@@ -42,14 +40,12 @@ typedef struct {
 #define SERVO1_US_MAX  2100
 #define SERVO1_DEG_BIAS 95.0f    // Calibrated to center gimbal at 0° when both servos are at 1500µs
 #define SERVO1_DEG_RANGE_MAX 120.0f
-#define SERVO1_REVERSED false
 
 /* Servo 2 (X-axis / roll): TIM3 CH3 / PB0 */
 #define SERVO2_US_MIN  900
 #define SERVO2_US_MAX  2100
 #define SERVO2_DEG_BIAS 50.0f   // Calibrated to center gimbal at 0° when both servos are at 1500µs
 #define SERVO2_DEG_RANGE_MAX 120.0f
-#define SERVO2_REVERSED false
 
 /* ── Gimbal angular limits ───────────────────────────────────────────── */
 #define SERVO_GIMBAL_DEG_MIN -30.0f 
@@ -69,8 +65,8 @@ void set_servo_degree(servo_t *servo, float degree);
 /**
  * @brief Command gimbal angles by physical axis.
  * Handles servo axis mapping and sign convention internally.
- * @param x_deg  Desired X-axis (roll) gimbal angle in degrees, clamped to ±SERVO_GIMBAL_HALF_RANGE_DEG.
- * @param y_deg  Desired Y-axis (pitch) gimbal angle in degrees, clamped to ±SERVO_GIMBAL_HALF_RANGE_DEG.
+ * @param x_deg  Desired X-axis (roll) gimbal angle in degrees, clamped to bewteen SERVO_GIMBAL_DEG_MIN and SERVO_GIMBAL_DEG_MAX.
+ * @param y_deg  Desired Y-axis (pitch) gimbal angle in degrees, clamped to bewteen SERVO_GIMBAL_DEG_MIN and SERVO_GIMBAL_DEG_MAX.
  */
 void set_gimbal_degrees(float x_deg, float y_deg);
 

embedded-software/firmware/ulysses-flight-controller/Core/Src/motor_drivers/servo_driver.c

@@ -20,7 +20,6 @@ static servo_config_t s_servo1_config = {
     .deg_min = SERVO_GIMBAL_DEG_MIN,
     .deg_max = SERVO_GIMBAL_DEG_MAX,
     .deg_bias = SERVO1_DEG_BIAS,
-    .reversed = SERVO1_REVERSED,
 };
 
 static servo_config_t s_servo2_config = {
@@ -30,7 +29,6 @@ static servo_config_t s_servo2_config = {
     .deg_min = SERVO_GIMBAL_DEG_MIN,
     .deg_max = SERVO_GIMBAL_DEG_MAX,
     .deg_bias = SERVO2_DEG_BIAS,
-    .reversed = SERVO2_REVERSED,
 };
 
 
@@ -162,11 +160,7 @@ static uint16_t degree_to_us(const servo_t *servo, float degree) {
     const servo_config_t *cfg = &servo->config;
 
     float scale = (float)(cfg->us_max - cfg->us_min) / cfg->deg_range_max;
-
-    // float d = (cfg->reversed ? -degree : degree) + cfg->deg_bias;
-    // float d_clamped = clamp_float(degree, cfg->deg_min, cfg->deg_max);
-
     uint16_t us = cfg->us_min + (degree + cfg->deg_bias) * scale;
-    // return clamp_u16(us, cfg->us_min, cfg->us_max);
+
     return us;
 }

embedded-software/firmware/ulysses-flight-controller/Core/Src/tasks/controls.c

@@ -27,7 +27,10 @@
 #include "timestamp.h"
 #include "utilities/clamp.h"
 
+/* Utility macros */
 #define RAD_TO_DEG (180.0f / 3.14159265f)
+#define DEG_TO_RAD (3.14159265f / 180.0f);
+#define SQRT_2_2 (0.70710678f) /**< For 45 degree trim to stay within circular gimbal limits. */
 
 #define CONTROLS_DT_S 0.00125f /**< Control period [s] (800 Hz via TIM4 CH2). */
 #define STALE_STATE_THRESHOLD_TICKS \
@@ -46,7 +49,6 @@
 #define ESC_HOLD_MS       500           /**< Hold at peak thrust (ms). */
 
 /* Empirical times for the ESC power on sequence*/
-#define ESC_POWER_ON_TIME_MS (3000) /**< Delay before arming sequence begins */
 #define ESC_ARM_TIME_MS      (3000) /**< Delay before PWM output */
 
 /* Empirical values for a safe operational range of the gimbal */
@@ -126,8 +128,6 @@ void controls_task_start(void *argument)
     servo_pair_enable(false);
     esc_pair_set_armed(false);
 
-    osDelay(pdMS_TO_TICKS(ESC_POWER_ON_TIME_MS));
-
     state_exchange_publish_startup_test_complete(true);
 
     for (;;) {
@@ -246,8 +246,8 @@ void controls_task_start(void *argument)
                 float theta_y_deg =
                     clamp_float(control_output.theta_y_cmd * RAD_TO_DEG, SERVO_MIN_DEGREES, SERVO_MAX_DEGREES);
 
-                float calibrated_theta_x_deg = (1/sqrtf(2.0f)) * theta_x_deg + (1/sqrtf(2.0f)) * theta_y_deg; // Calibrated to match physical gimbal direction
-                float calibrated_theta_y_deg = -(1/sqrtf(2.0f)) * theta_x_deg + (1/sqrtf(2.0f)) * theta_y_deg; // Calibrated to match physical gimbal direction
+                float calibrated_theta_x_deg = (SQRT_2_2) * theta_x_deg + (SQRT_2_2) * theta_y_deg; // Calibrated to match physical gimbal direction
+                float calibrated_theta_y_deg = -(SQRT_2_2) * theta_x_deg + (SQRT_2_2) * theta_y_deg; // Calibrated to match physical gimbal direction
 
                 set_gimbal_degrees(calibrated_theta_x_deg, calibrated_theta_y_deg);
                 esc_pair_set_force(control_output.T_cmd, control_output.tau_thrust);