Trim Tab Angle Scaling (#501)

* Updated trim tab scaling

* Flake 8 updates

* Changed scaling function

* Updated upper wind threshold

Author: kristatraboulay

src/controller/controller/wingsail/wingsail_ctrl_node.py

@@ -15,8 +15,6 @@
 from controller.common.lut import LUT
 from controller.wingsail.controllers import WingsailController
 
-SCALING_INTERCEPT = 1
-
 
 def main(args=None):
     rclpy.init(args=args)
@@ -78,8 +76,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),
+                ("apparent_wind_lower_threshold", rclpy.Parameter.Type.DOUBLE),
+                ("apparent_wind_upper_threshold", rclpy.Parameter.Type.DOUBLE),
             ],
         )
 
@@ -146,20 +144,26 @@ def __publish(self):
 
         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
+        apparent_lower_threshold = (
+            self.get_parameter("apparent_wind_lower_threshold").get_parameter_value().double_value
+        )
+        apparent_upper_threshold = (
+            self.get_parameter("apparent_wind_upper_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(
             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
-            )
+
+        # Gets scaling factor based on wind speed thresholds
+        scaling_coef = 1
+        if apparent_speed > apparent_lower_threshold and apparent_speed < apparent_upper_threshold:
+            difference = apparent_upper_threshold - apparent_lower_threshold
+            scaling_coef = -1 * (apparent_speed - apparent_lower_threshold) / difference + 1
+        elif apparent_speed >= apparent_upper_threshold:
+            scaling_coef = 0
+
+        self.__trim_tab_angle = scaling_coef * self.__trim_tab_angle
 
         msg.trim_tab_angle_degrees = self.__trim_tab_angle
 

src/global_launch/config/README.md

@@ -91,15 +91,15 @@ ROS parameters specific to the nodes in the Controller.
 - _Datatype_: `double`
 - _Range_: `(-180.0, 180.0]`
 
-**`apparent_wind_threshold`**
+**`apparent_wind_lower_threshold`**
 
-- _Description_: The high wind threshold value for apparent wind.
+- _Description_: The lower wind threshold value for apparent wind.
 - _Datatype_: 'double'
 - _Range_: '[0.0, MAX_DOUBLE)'
 
-**`scaling_coef`**
+**`apparent_wind_upper_threshold`**
 
-- _Description_: The coefficient used to scale trim tab angle.
+- _Description_: The higher wind threshold value for apparent wind.
 - _Datatype_: 'double'
 - _Range_: '[0.0, MAX_DOUBLE)'
 

src/global_launch/config/globals.yaml

@@ -21,8 +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
+        apparent_wind_lower_threshold: 10.0
+        apparent_wind_upper_threshold: 15.0
 
 # boat_simulator parameters
 low_level_control_node: