diff --git a/riptide_controllers/models/complete_system/complete_controller.slx b/riptide_controllers/models/complete_system/complete_controller.slx index 1c650a9..7f20d4a 100644 Binary files a/riptide_controllers/models/complete_system/complete_controller.slx and b/riptide_controllers/models/complete_system/complete_controller.slx differ diff --git a/riptide_controllers/models/referenced_models/AppliedForceScale.slx b/riptide_controllers/models/referenced_models/AppliedForceScale.slx index 0a86cb1..2b16369 100644 Binary files a/riptide_controllers/models/referenced_models/AppliedForceScale.slx and b/riptide_controllers/models/referenced_models/AppliedForceScale.slx differ diff --git a/riptide_controllers/models/referenced_models/Auto_FF_Scale_Processor.slx b/riptide_controllers/models/referenced_models/Auto_FF_Scale_Processor.slx index 2821bd8..f0b1d0e 100644 Binary files a/riptide_controllers/models/referenced_models/Auto_FF_Scale_Processor.slx and b/riptide_controllers/models/referenced_models/Auto_FF_Scale_Processor.slx differ diff --git a/riptide_controllers/models/referenced_models/Diagnostic_Reporter.slx b/riptide_controllers/models/referenced_models/Diagnostic_Reporter.slx index 7cee3ef..b15a7d6 100644 Binary files a/riptide_controllers/models/referenced_models/Diagnostic_Reporter.slx and b/riptide_controllers/models/referenced_models/Diagnostic_Reporter.slx differ diff --git a/riptide_controllers/models/referenced_models/Thruster_Solver_Mathmatical.slx b/riptide_controllers/models/referenced_models/Thruster_Solver_Mathmatical.slx index e4e8d1a..0d766ca 100644 Binary files a/riptide_controllers/models/referenced_models/Thruster_Solver_Mathmatical.slx and b/riptide_controllers/models/referenced_models/Thruster_Solver_Mathmatical.slx differ diff --git a/riptide_controllers/src/riptide_controllers/controller_overseer.py b/riptide_controllers/src/riptide_controllers/controller_overseer.py index 7cc8502..8e794dd 100644 --- a/riptide_controllers/src/riptide_controllers/controller_overseer.py +++ b/riptide_controllers/src/riptide_controllers/controller_overseer.py @@ -4,6 +4,7 @@ import yaml import yaml.parser import rclpy +import shutil import numpy as np from collections import deque from rclpy.node import Node @@ -45,7 +46,7 @@ AUTOTUNE_REINIT_TOPIC_NAME = "controller/re_init_accumulators" WEIGHTS_FORCE_UPDATE_PERIOD = 1 -ORIN_AUTOTUNE_DIR = "/bin" +ORIN_AUTOTUNE_DIR = "/var/lib/autoff" AUTOFF_INIT_TOLERANCE = .01 #seeing rounding errors lol @@ -902,6 +903,13 @@ def ffAutoTuneCB(self, msg): self.re_init_signal_pub.publish(msg) return + + # Make sure we aren't writing 0 -ish values + if (abs(msg.linear.x) < AUTOFF_INIT_TOLERANCE and abs(msg.linear.y) < AUTOFF_INIT_TOLERANCE and + abs(msg.linear.z) < AUTOFF_INIT_TOLERANCE and abs(msg.angular.x) < AUTOFF_INIT_TOLERANCE and + abs(msg.angular.y) < AUTOFF_INIT_TOLERANCE and abs(msg.angular.z) < AUTOFF_INIT_TOLERANCE): + self.get_logger().warn("Received near-zero autotune data, skipping write to prevent corruption") + return #if the twist has been updated if (not (self.currentAutoTuneTwist[0] == msg.linear.x and self.currentAutoTuneTwist[1] == msg.linear.y and self.currentAutoTuneTwist[2] == msg.linear.z and @@ -911,7 +919,7 @@ def ffAutoTuneCB(self, msg): drag_forward_string = "" drag_reverse_string = "" - if(not ((self.drag_comp_forward_data is None) or (self.drag_comp_reverse_data is None))): + if self.drag_comp_forward_data is not None and self.drag_comp_reverse_data is not None: #write the forward drag string drag_forward_string = f"drag_forward: [" @@ -927,22 +935,25 @@ def ffAutoTuneCB(self, msg): #write config to file try: - with open(self.autoff_config_path, "w") as config: - + # Write to temporary file first + temp_path = self.autoff_config_path + ".tmp" + with open(temp_path, "w") as config: config.write(auto_ff_config_string) - - - if not ((self.drag_comp_forward_data is None) or (self.drag_comp_reverse_data is None)): - #cant save until cb complete + if self.drag_comp_forward_data is not None and self.drag_comp_reverse_data is not None: config.write(drag_forward_string) config.write(drag_reverse_string) - - config.close() - - except FileExistsError: - self.get_logger().error(f"Cannot open ff auto tune file at: {self.autoff_config_path}") - except PermissionError: - self.get_logger().error(f"No Permission to write of autoff file at: {self.autoff_config_path}") + + # Move temp only after successful write + shutil.move(temp_path, self.autoff_config_path) + + except (FileExistsError, PermissionError) as e: + # Clean up temp file if it exists + if os.path.exists(temp_path): + try: + os.remove(temp_path) + except: + pass + self.get_logger().error(f"Failed to write autotune file: {e}")