Force to pwm (#635)

* Modified jetson compose file, removed launch command

* defined hamilton product

* flipped about x - ultimately flipping z

* modified thruster values - to be tested

catkin_ws/src/propulsion/src/thrust_mapper_utils.py

@@ -2,7 +2,7 @@
 MAX_FWD_FORCE = 4.52 * 9.81
 MAX_BKWD_FORCE = -3.52 * 9.81
 
-thruster_mount_dirs = [1, -1, -1, 1, 1, 0.5, 0.5, 1]
+thruster_mount_dirs = [1, -1, -1, 1, -1, 0.5, -0.5, 1]
 
 def force_to_pwm(force):
     """

catkin_ws/src/sensors/src/waterlinked_dvl.py

@@ -8,6 +8,17 @@
 
 RAD_PER_DEG = np.pi / 180.0
 
+def hamilton_product(q1,q2):
+    w1,x1,y1,z1 = q1 #define first rotation
+    w2,x2,y2,z2 = q2 #define second rotation
+
+    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
+    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
+    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
+    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
+    return [w, x, y, z]
+#this function will rotate the DVLs orientation 180 degrees about x axis(whcih flips the z axis)
+
 def parse_velocity_report(line):
     tokens = line.split(",")
     vx = float(tokens[1])
@@ -61,10 +72,14 @@ def parse_dead_reckon_report(line, quat_variance):
     report.pose.pose.position.z = z
 
     quaternion = transformations.quaternion_from_euler(roll * RAD_PER_DEG, pitch * RAD_PER_DEG, yaw * RAD_PER_DEG)
-    report.pose.pose.orientation.x = quaternion[0]
-    report.pose.pose.orientation.y = quaternion[1]
-    report.pose.pose.orientation.z = quaternion[2]
-    report.pose.pose.orientation.w = quaternion[3]
+    correction_quat = [0, 1, 0, 0]  #flip about x
+    corrected_quat = hamilton_product(quaternion, correction_quat)
+
+
+    report.pose.pose.orientation.x = corrected_quat[0]
+    report.pose.pose.orientation.y = corrected_quat[1]
+    report.pose.pose.orientation.z = corrected_quat[2]
+    report.pose.pose.orientation.w = corrected_quat[3]
     report.pose.covariance = [0.0] * 36
     for i in range(0,3):
         report.pose.covariance[i * 6 + i] = std