fixes from easter testing

vortexntnu · Apr 3, 2024 · 31012b6 · 31012b6
1 parent 82bbf76
commit 31012b6
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 14 deletions.
diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml
@@ -37,13 +37,13 @@
         configuration_matrix: #NED
           [0.70711, 0.70711, 0.70711, 0.70711, 
           -0.70711, 0.70711, -0.70711, 0.70711, 
-          0.27738, 0.27738, -0.27738, -0.27738]
+          -0.8485, -0.8485, 0.8485, 0.8485]
 
         rate_of_change:
           max: 0 # Maximum rate of change in newton per second for a thruster
 
         thruster_to_pin_mapping: [3, 2, 1, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc..
-        thruster_direction: [1,-1,-1, 1] # Disclose during thruster mapping (+/- 1)
+        thruster_direction: [-1, 1, 1,-1] # Disclose during thruster mapping (+/- 1)
         thruster_PWM_offset: [0, 0, 0, 0] # Disclose during thruster mapping, Recomended: [0, 0, 0, 0]
         thruster_PWM_min: [1100, 1100, 1100, 1100] # Minimum PWM value, Recomended: [1100, 1100, 1100, 1100]
         thruster_PWM_max: [1900, 1900, 1900, 1900] # Maximum PWM value, Recomended: [1900, 1900, 1900, 1900]

diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py
@@ -37,9 +37,9 @@ def generate_launch_description():
                    '--z'              , '-0.392425',
                    '--roll'           , str(math.pi),
                    '--pitch'          , '0',
-                   '--yaw'            , '0',
+                   '--yaw'            , str(math.pi),
                    '--frame-id'       , 'base_link',
-                   '--child-frame-id' , 'os_lidar'],
+                   '--child-frame-id' , 'os_sensor'],
     )
 
     # base_link to zed2i_camera_center tf.

diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp
@@ -192,6 +192,15 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping,
 
 // The main core functionality of interacting and controling the thrusters
 int16_t *drive_thrusters(float *thrusterForces) {
+
+  // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to
+  // the direction parameter
+  float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0};
+  for (int8_t i = 0; i < 4; i++) {
+    thrusterForcesChangedDirection[i] =
+        thrusterForces[i] * _thrusterDirection[i];
+  }
+
   // Remap Thrusters
   // From: [pin1:thruster1, pin2:thruster2, pin3:thruster3, pin4:thruster4]
   // To:   [pin1:<specifiedThruster>, pin2:<specifiedThruster>,
@@ -200,19 +209,11 @@ int16_t *drive_thrusters(float *thrusterForces) {
   for (int8_t pinNr = 0; pinNr < 4; pinNr++) {
     int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr];
     thrusterForcesChangedMapping[pinNr] =
-        thrusterForces[remapedThrusterForcesIndex];
-  }
-
-  // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to
-  // the direction parameter
-  float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0};
-  for (int8_t i = 0; i < 4; i++) {
-    thrusterForcesChangedDirection[i] =
-        thrusterForcesChangedMapping[i] * _thrusterDirection[i];
+        thrusterForcesChangedDirection[remapedThrusterForcesIndex];
   }
 
   // Interpolate forces to raw PWM values
-  int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedDirection);
+  int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedMapping);
 
   // Offset PWM
   for (int8_t i = 0; i < 4; i++) {