diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 8eb7838c..8eab0af8 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -50,6 +50,9 @@ scaling: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] blackbox: - data_logging_rate: 5.0 # Log all data 5 times per second + data_logging_rate: 5.0 # [logings/second], Recomended: 5.0 logings per second + + internal_status: + psm_read_rate: 2.0 # [readings/second], Recomended 2.0 readings per second OBS! Try keeping under 2.0 as otherwise I2C comunication to Power Sense Module might become bad as psm needs time to sample data computer: "pc-debug" diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index ec6c9acf..cfbf9307 100644 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -71,9 +71,10 @@ def __init__(self): ) # Logs all the newest data 10 times per second - self.declare_parameter("blackbox.data_logging_rate", 0.1) # Providing a default value + self.declare_parameter("blackbox.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow DATA_LOGING_RATE = self.get_parameter("blackbox.data_logging_rate").get_parameter_value().double_value - self.logger_timer = self.create_timer(1.0/DATA_LOGING_RATE, self.logger) + timer_period = 1.0/DATA_LOGING_RATE + self.logger_timer = self.create_timer(timer_period, self.logger) # Debuging ---------- self.get_logger().info( diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index f133e639..a7b54bce 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -5,7 +5,6 @@ def generate_launch_description(): # Path to the YAML file - # If the YAML file is not within a package, specify the absolute path directly yaml_file_path = os.path.join( get_package_share_directory("blackbox"), "../../../../", # Go to the workspace diff --git a/mission/internal_status/internal_status/__init__.py b/mission/internal_status/internal_status/__init__.py old mode 100644 new mode 100755 diff --git a/mission/internal_status/internal_status/power_sense_module_lib.py b/mission/internal_status/internal_status/power_sense_module_lib.py old mode 100644 new mode 100755 index 4cb4f956..2036c1a2 --- a/mission/internal_status/internal_status/power_sense_module_lib.py +++ b/mission/internal_status/internal_status/power_sense_module_lib.py @@ -1,3 +1,4 @@ +import time import smbus from MCP342x import MCP342x @@ -11,6 +12,7 @@ def __init__(self): # init of I2C bus communication self.bus = smbus.SMBus(1) + time.sleep(1) # A short pause because sometimes I2C is slow to conect self.channel_voltage = MCP342x(self.bus, self.i2c_adress, channel=1, @@ -18,7 +20,7 @@ def __init__(self): self.channel_current = MCP342x(self.bus, self.i2c_adress, channel=0, - resolution=18) # current + resolution=18) # current # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/ @@ -35,7 +37,7 @@ def get_voltage(self): return system_voltage except IOError as e: - return -1 + return e def get_current(self): try: @@ -46,7 +48,7 @@ def get_current(self): return system_current except IOError as e: - return -1 + return e def shutdown(self): self.bus.close() diff --git a/mission/internal_status/internal_status/power_sense_module_publisher.py b/mission/internal_status/internal_status/power_sense_module_publisher.py old mode 100644 new mode 100755 index b1a44454..4f6bbe24 --- a/mission/internal_status/internal_status/power_sense_module_publisher.py +++ b/mission/internal_status/internal_status/power_sense_module_publisher.py @@ -8,11 +8,17 @@ class MinimalPublisher(Node): def __init__(self): + # Node setup ---------- super().__init__('PSM_publisher') self.PSM = internal_status.power_sense_module_lib.PowerSenseModule() + self.publisher_current = self.create_publisher(Float32, '/asv/power_sense_module/current', 5) self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 5) - timer_period = 0.5 + + # Data gathering cycle ---------- + self.declare_parameter("internal_status.psm_read_rate", 1.0) # Providing a default value 1.0 => 1 second delay per data gathering + psm_read_rate = self.get_parameter("internal_status.psm_read_rate").get_parameter_value().double_value + timer_period = 1.0/psm_read_rate self.timer = self.create_timer(timer_period, self.timer_callback) diff --git a/mission/internal_status/launch/internal_status_launch.py b/mission/internal_status/launch/internal_status_launch.py index 18c26015..f29fa153 100644 --- a/mission/internal_status/launch/internal_status_launch.py +++ b/mission/internal_status/launch/internal_status_launch.py @@ -1,10 +1,22 @@ from launch import LaunchDescription from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + return LaunchDescription([ Node( package = 'internal_status', - executable = 'power_sense_module_publisher' + executable = 'power_sense_module_publisher', + output='screen', + parameters=[yaml_file_path], ) ]) \ No newline at end of file diff --git a/mission/internal_status/setup.py b/mission/internal_status/setup.py index 8f4ee25f..71179627 100644 --- a/mission/internal_status/setup.py +++ b/mission/internal_status/setup.py @@ -3,6 +3,7 @@ from setuptools import find_packages, setup package_name = 'internal_status' +yaml_file_dir = "asv_setup/" setup( name=package_name, @@ -12,7 +13,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', yaml_file_dir, 'config/robots'), glob('robots/*.yaml')), ], install_requires=['setuptools'], zip_safe=True,