Merge branch 'feature/njord' of https://github.com/vortexntnu/vortex-asv

into feature/njord
vortexntnu · Aug 13, 2023 · 524cb12 · 524cb12
2 parents dc7787e + 3bcfdc5
commit 524cb12
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/mission/finite_state_machine/scripts/lqr_interface.py b/mission/finite_state_machine/scripts/lqr_interface.py
@@ -39,13 +39,16 @@ def set_path_dependent_heading(self, data):
     def clear_all_waypoints(self):
         self.clear_waypoints_publisher.publish(Empty())
 
-    def north_east_displacement_in_meters(target_north, target_east, origin_north, origin_east):
+    def north_east_displacement_in_meters(target_north, target_east,
+                                          origin_north, origin_east):
         earth_radius_wgs84 = 6371 * 1000.0
 
         meter_per_degree_lat = earth_radius_wgs84 * np.pi / 180.0
-        meter_per_degree_lon = meter_per_degree_lat * np.cos(origin_north * np.pi / 180.0)
+        meter_per_degree_lon = meter_per_degree_lat * np.cos(
+            origin_north * np.pi / 180.0)
 
-        displacement_north = (target_north - origin_north) * meter_per_degree_lat
+        displacement_north = (target_north -
+                              origin_north) * meter_per_degree_lat
         displacement_east = (target_east - origin_east) * meter_per_degree_lon
 
         return displacement_north, displacement_east