use config parameter to decide thrust update rate

vortexntnu · Jun 5, 2024 · 7b69a74 · 7b69a74
1 parent 926d2f7
commit 7b69a74
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml
@@ -39,7 +39,7 @@
         rate_of_change:
           max: 1 # Maximum rate of change in newton per second for a thruster
 
-        thrust_update_rate: 50.0 # [Hz]
+        thrust_update_rate: 10.0 # [Hz]
 
         thruster_to_pin_mapping: [0, 1, 2, 3, 4, 6, 5, 7] # I.e. if thruster_to_pin = [ 7,  6,  5,  4,  3,  2,  1,  0 ] then thruster 0 is pin 1 etc..
         thruster_direction: [1, -1, 1, -1, 1, 1, 1, -1]  # Disclose during thruster mapping (+/- 1)

diff --git a/motion/thruster_allocator_auv/src/thruster_allocator_ros.cpp b/motion/thruster_allocator_auv/src/thruster_allocator_ros.cpp
@@ -16,6 +16,7 @@ ThrusterAllocator::ThrusterAllocator()
   declare_parameter("propulsion.thrusters.num", 8);
   declare_parameter("propulsion.thrusters.min", -100);
   declare_parameter("propulsion.thrusters.max", 100);
+  declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0)
   declare_parameter("propulsion.thrusters.thruster_force_direction",
                     std::vector<double>{0});
   declare_parameter("propulsion.thrusters.thruster_position",
@@ -26,6 +27,7 @@ ThrusterAllocator::ThrusterAllocator()
   num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int();
   min_thrust_ = get_parameter("propulsion.thrusters.min").as_int();
   max_thrust_ = get_parameter("propulsion.thrusters.max").as_int();
+  thrust_update_period_ = std::chrono::milliseconds(1000 / get_parameter("propulsion.thrusters.thrust_update_rate").as_double());
 
   thruster_force_direction_ = double_array_to_eigen_matrix(
       get_parameter("propulsion.thrusters.thruster_force_direction")
@@ -48,7 +50,7 @@ ThrusterAllocator::ThrusterAllocator()
           "thrust/thruster_forces", 5);
 
   calculate_thrust_timer_ = this->create_wall_timer(
-      100ms, std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));
+      thrust_update_period_, std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));
 
   pseudoinverse_allocator_.T_pinv =
       calculate_right_pseudoinverse(thrust_configuration);

diff --git a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py
@@ -38,6 +38,8 @@ def __init__(self):
         self.declare_parameter(
             'propulsion.thrusters.thruster_PWM_max',
             [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900])
+
+        self.declare_parameter('propulsion.thrusters.thrust_update_rate', 10.0)
 
         self.thruster_mapping = self.get_parameter(
             'propulsion.thrusters.thruster_to_pin_mapping').value
@@ -49,6 +51,7 @@ def __init__(self):
             'propulsion.thrusters.thruster_PWM_min').value
         self.thruster_PWM_max = self.get_parameter(
             'propulsion.thrusters.thruster_PWM_max').value
+        self.thrust_timer_period = 1.0 / self.get_parameter('propulsion.thrusters.thrust_update_rate').value
 
         # Initialize thruster driver
         self.thruster_driver = ThrusterInterfaceAUVDriver(
@@ -63,7 +66,7 @@ def __init__(self):
         # Start clock timer for driving thrusters every 0.2 seconds
         # Declare "self.thruster_forces_array" in case no topic comes in at the first possible second
         self.thruster_forces_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        self.timer = self.create_timer(0.2, self._timer_callback)
+        self.timer = self.create_timer(self.thrust_timer_period, self._timer_callback)
 
         # Debugging
         self.get_logger().info(