1. Minor code refactoring

2. Updated the TCM to match BlueRobotics' Blue Rov Heavy configuration
3. Uniformed the order for cmd_vel to surge, sway, heave, roll, pitch, yaw in all the functions and libraries.
4. Fixed a bug in the update_setpoints function, I miscalculated the array index which was off by 1
5. Uniformed the reference system to match the one used by the most literature on underwater robotics.

Author: Z4nna

Core/Inc/navigation/ThrusterConfigurationMatrix.h

@@ -25,8 +25,8 @@ extern "C" {
 #include "arm_math.h"
 
 /**
- * The fixed 8x6 mixing matrix: the columns are, respectively: surge, sway, heave, pitch, roll, yaw.
- * The rows are, respectively, thrusters from 0 to 7.
+ * The fixed 8x6 mixing matrix: the columns are, respectively: surge, sway, heave, roll, pitch, yaw.
+ * The rows are, respectively, thrusters from 1 to 8. Refer to BlueRobotics' Blue ROV Heavy configuration.
  */
 __attribute__((aligned(4))) extern float FIXED_MIXING_MATRIX[8][6];
 

Core/Inc/navigation/navigation.h

@@ -44,8 +44,8 @@ typedef struct
 /**
  * @brief Calculates PWM output values from command velocity
  *
- * @param cmd_vel
- * @param pwm_output
+ * @param cmd_vel: surge, sway, heave, roll, pitch, yaw
+ * @param pwm_output: thrusters 1 to 8
  * @return eventual error code
  */
 arm_status calculate_pwm(const float cmd_vel[6], uint32_t pwm_output[8]);

Core/Inc/navigation/stabilize_mode.h

@@ -38,8 +38,8 @@ void init_pids(float kps[PID_NUMBER], float kis[PID_NUMBER], float kds[PID_NUMBE
 /**
  * @brief Calculates the PWM output using PID control to adjust orientation and depth based on joystick input.
  *
- * @param joystick_input Array of 6 cmd velocity values: [surge, sway, heave, pitch, roll, yaw].
- * @param pwm_output Array to store the 8 calculated PWM output values.
+ * @param joystick_input Array of 6 cmd velocity values: [surge, sway, heave, roll, pitch, yaw].
+ * @param pwm_output Array to store the 8 calculated PWM output values (thrusters 1 to 8, referring to BlueRobotics' Blue Rov Heavy)
  * @param orientation_quaternion Pointer to the current orientation as a quaternion.
  * @param water_pressure Pointer to the current water pressure (used to estimate depth).
  * @return int8_t A status code indicating the success or failure of the PWM calculation.

Core/Src/freertos.cpp

@@ -269,13 +269,10 @@ void StartDefaultTask(void *argument)
 	    			break;
 	    	}
 	    	clamp_pwm_output(pwm_output, 8);
-	    	// offset to compensate for the optocoupler offset
-	    	for(int8_t i = 0; i < 8; i++) pwm_output[i] += OPTOCOUPLER_INTRODUCED_OFFSET_uS;
 	    	set_pwms(pwm_output);
 	    } else set_pwm_idle();
 
-	    // do not publish the offset by subtracting 50
-	    for(uint8_t i = 0; i < 8; i++) thruster_status_msg.thruster_pwms[i] = pwm_output[i] - OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	    for(uint8_t i = 0; i < 8; i++) thruster_status_msg.thruster_pwms[i] = pwm_output[i];
 	    rc = rcl_publish(&thruster_status_publisher, &thruster_status_msg, NULL);
 	    if(rc!=RCL_RET_OK) printf("Error publishing (line %d)\n", __LINE__);
 
@@ -289,29 +286,27 @@ void StartDefaultTask(void *argument)
 /* USER CODE BEGIN Application */
 void inline set_pwms(uint32_t pwms[8])
 {
-	TIM2 -> CCR1 = pwms[0];
-	TIM2 -> CCR2 = pwms[1];
-	TIM2 -> CCR3 = pwms[2];
-	TIM2 -> CCR4 = pwms[3];
-
-	// vertical thrusters
-	TIM3 -> CCR1 = pwms[4];
-	TIM3 -> CCR2 = pwms[5];
-	TIM3 -> CCR3 = pwms[6];
-	TIM3 -> CCR4 = pwms[7];
+	// HERE THE PWM Channel - Thruster relation is defined
+	TIM2 -> CCR1 = pwms[0] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR2 = pwms[1] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR3 = pwms[2] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR4 = pwms[3] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	// VERTICAL THRUSTERS
+	TIM3 -> CCR1 = pwms[4] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR2 = pwms[5] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR3 = pwms[6] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR4 = pwms[7] + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
 }
 void inline set_pwm_idle()
 {
-	TIM2 -> CCR1 = PWM_IDLE;
-	TIM2 -> CCR2 = PWM_IDLE;
-	TIM2 -> CCR3 = PWM_IDLE;
-	TIM2 -> CCR4 = PWM_IDLE;
-
-	// vertical thrusters
-	TIM3 -> CCR1 = PWM_IDLE;
-	TIM3 -> CCR2 = PWM_IDLE;
-	TIM3 -> CCR3 = PWM_IDLE;
-	TIM3 -> CCR4 = PWM_IDLE;
+	TIM2 -> CCR1 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR2 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR3 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM2 -> CCR4 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR1 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR2 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR3 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
+	TIM3 -> CCR4 = PWM_IDLE + OPTOCOUPLER_INTRODUCED_OFFSET_uS;
 }
 void clamp_pwm_output(uint32_t pwms[], int N) {
 	for(uint16_t i = 0; i < N; i++) {
@@ -331,16 +326,14 @@ void update_pid_constants(arm_pid_instance_f32 *pid, float32_t Kp, float32_t Ki,
     pid->A2 = Kd;
 }
 void imu_subscription_callback(const void * msgin) {
-	const sensor_msgs__msg__Imu * msg = (const sensor_msgs__msg__Imu *)msgin;
+
 }
 void cmd_vel_subscription_callback (const void * msgin) {
 
 }
 void arm_disarm_service_callback(const void * request_msg, void * response_msg) {
 	std_srvs__srv__SetBool_Request * req_in = (std_srvs__srv__SetBool_Request *) request_msg;
 	std_srvs__srv__SetBool_Response * res_in = (std_srvs__srv__SetBool_Response *) response_msg;
-
-	// Handle request message and set the response message values
 	rov_arm_mode = req_in->data ? ROV_ARMED : ROV_DISARMED;
 	printf("%d: arm mode.\n", (int)rov_arm_mode);
 	res_in->success = true;
@@ -351,9 +344,7 @@ void arm_disarm_service_callback(const void * request_msg, void * response_msg)
 void set_nav_mode_service_callback(const void * request_msg, void * response_msg) {
 	nereo_interfaces__srv__SetNavigationMode_Request * req_in = (nereo_interfaces__srv__SetNavigationMode_Request *) request_msg;
 	nereo_interfaces__srv__SetNavigationMode_Response * res_in = (nereo_interfaces__srv__SetNavigationMode_Response *) response_msg;
-
 	navigation_mode = (NavigationModes)req_in->navigation_mode;
-
 	res_in->mode_after_set = navigation_mode;
 	res_in->success = true;
 }

Core/Src/navigation/ThrusterConfigurationMatrix.c

@@ -8,13 +8,13 @@
 #include <navigation/ThrusterConfigurationMatrix.h>
 
 __attribute__((aligned(4))) float FIXED_MIXING_MATRIX[8][6] = {
+    {0.7071067812, -0.7071067812, 0, 0, 0, -1},
     {0.7071067812, 0.7071067812, 0, 0, 0, 1},
-    {-0.7071067812, 0.7071067812, 0, 0, 0, -1},
-    {-0.7071067812, 0.7071067812, 0, 0, 0, -1},
-    {0.7071067812, 0.7071067812, 0, 0, 0, 1},
-    {0, 0, 1, 1, -1, 0},
-    {0, 0, 1, 1, 1, 0},
+    {0.7071067812, 0.7071067812, 0, 0, 0, -1},
+    {0.7071067812, -0.7071067812, 0, 0, 0, 1},
+    {0, 0, -1, -1, 1, 0},
     {0, 0, 1, -1, -1, 0},
-    {0, 0, 1, -1, 1, 0}
+    {0, 0, 1, 1, 1, 0},
+    {0, 0, -1, 1, -1, 0}
 };
 

Core/Src/navigation/navigation.c

@@ -9,19 +9,10 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-/**
- * @brief Calculates the PWM output values based on the joystick input.
- *
- * This function takes the cmd_vel values, normalizes them, and then
- * multiplies them with a fixed mixing matrix to generate the PWM output values.
- * The PWM output values are then normalized and mapped to the range of PWM_MIN
- * to PWM_MAX.
- *
- * @param joystick_input An array of 6 float values representing the joystick input.
- * @param pwm_output An array of 8 uint16_t values to store the calculated PWM output.
- * @return OK if the calculation was successful, MAT_MULT_ERROR if the matrix
- * multiplication failed.
- */
+
+static uint8_t tcm_initialized = 0;
+static arm_matrix_instance_f32 fixed_mixing_matrix_instance;
+
 arm_status calculate_pwm(const float in_joystick_input[6], uint32_t pwm_output[8])
 {
 	float joystick_input[6];
@@ -34,13 +25,15 @@ arm_status calculate_pwm(const float in_joystick_input[6], uint32_t pwm_output[8
 
     float f_pwm_output[8];
 
-    __attribute__((aligned(4))) float pwm_output_8_1[8][1] = {0};
+    __attribute__((aligned(4))) float pwm_output_8_1[8] = {0};
 
-    arm_matrix_instance_f32 fixed_mixing_matrix_instance;
     arm_matrix_instance_f32 joystick_input_instance;
     arm_matrix_instance_f32 pwm_output_instance;
 
-    arm_mat_init_f32(&fixed_mixing_matrix_instance, 8, 6, (float *)FIXED_MIXING_MATRIX);
+    if(tcm_initialized == 0) {
+    	arm_mat_init_f32(&fixed_mixing_matrix_instance, 8, 6, (float *)FIXED_MIXING_MATRIX);
+    	tcm_initialized = 1;
+    }
     arm_mat_init_f32(&joystick_input_instance, 6, 1, (float *)joystick_input);
     arm_mat_init_f32(&pwm_output_instance, 8, 1, (float *)pwm_output_8_1);
     arm_status code = arm_mat_mult_f32(&fixed_mixing_matrix_instance, &joystick_input_instance, &pwm_output_instance);