Update Trim Tab Angle (#488)

* Added parameters, modified publisher callback

* Added inverse exponential, updated yaml file

* Fixed Flake8 error

* Specified parameter datatypes, updated ROS parameter README

* README syntax fixes

* Updated scaling function and README

Author: kristatraboulay

src/controller/controller/wingsail/wingsail_ctrl_node.py

@@ -15,6 +15,8 @@
 from controller.common.lut import LUT
 from controller.wingsail.controllers import WingsailController
 
+SCALING_INTERCEPT = 1
+
 
 def main(args=None):
     rclpy.init(args=args)
@@ -76,6 +78,8 @@ def __declare_ros_parameters(self):
                 ("pub_period_sec", rclpy.Parameter.Type.DOUBLE),
                 ("reynolds_number", rclpy.Parameter.Type.DOUBLE_ARRAY),
                 ("angle_of_attack", rclpy.Parameter.Type.DOUBLE_ARRAY),
+                ("apparent_wind_threshold", rclpy.Parameter.Type.DOUBLE),
+                ("scaling_coef", rclpy.Parameter.Type.DOUBLE),
             ],
         )
 
@@ -140,9 +144,22 @@ def __publish(self):
         It also logs information about the publication to the logger."""
         msg = SailCmd()
 
+        apparent_speed = self.__filtered_wind_sensor.speed.speed
+        apparent_direction = self.__filtered_wind_sensor.direction
+        apparent_threshold = (
+            self.get_parameter("apparent_wind_threshold").get_parameter_value().double_value
+        )
+
+        # Sets trim tab angle, scales if apparent wind speed is above threshold
         self.__trim_tab_angle = self.__wingsailController.get_trim_tab_angle(
-            self.__filtered_wind_sensor.speed.speed, self.__filtered_wind_sensor.direction
+            apparent_speed, apparent_direction
         )
+        if apparent_speed > apparent_threshold:
+            coef = self.get_parameter("scaling_coef").get_parameter_value().double_value
+            speed_difference = apparent_threshold - apparent_speed
+            self.__trim_tab_angle = (
+                self.__trim_tab_angle * coef * speed_difference + SCALING_INTERCEPT
+            )
 
         msg.trim_tab_angle_degrees = self.__trim_tab_angle
 

src/global_launch/config/README.md

@@ -91,6 +91,18 @@ ROS parameters specific to the nodes in the Controller.
 - _Datatype_: `double`
 - _Range_: `(-180.0, 180.0]`
 
+**`apparent_wind_threshold`**
+
+- _Description_: The high wind threshold value for apparent wind.
+- _Datatype_: 'double'
+- _Range_: '[0.0, MAX_DOUBLE)'
+
+**`scaling_coef`**
+
+- _Description_: The coefficient used to scale trim tab angle.
+- _Datatype_: 'double'
+- _Range_: '[0.0, MAX_DOUBLE)'
+
 ## Boat Simulator Parameters
 
 ROS parameters specific to the nodes in the boat simulator.

src/global_launch/config/globals.yaml

@@ -21,6 +21,8 @@ wingsail_ctrl_node:
     ros__parameters:
         reynolds_number: [0.0, 1.0, 2.0]
         angle_of_attack: [0.0, 1.0, 2.0]
+        apparent_wind_threshold: 10.0
+        scaling_coef: 1.0
 
 # boat_simulator parameters
 low_level_control_node: