just testing smbus file

mission/internal_status/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(internal_status)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mission/internal_status/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>internal_status</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">rose</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mission/internal_status/src/battery_monitor.py

@@ -0,0 +1,175 @@
+#!/usr/bin/python3
+import time
+import rospy
+import smbus
+
+from MCP342x import MCP342x
+from std_msgs.msg import Float32
+
+
+class BatteryMonitor:
+
+    def __init__(self):
+        rospy.init_node("battery_monitor") ###########NODE
+
+        # Parameters
+        # to read voltage and current from ADC on PDB through I2C
+        self.i2c_adress = 0x69     ###########DRIVER
+
+        # init of I2C bus communication
+        self.bus = smbus.SMBus(1)
+        self.channel_voltage = MCP342x(self.bus,
+                                       self.i2c_adress,
+                                       channel=0,
+                                       resolution=18)  # voltage
+        self.channel_current = MCP342x(self.bus,
+                                       self.i2c_adress,
+                                       channel=1,
+                                       resolution=18)  # current   ###########DRIVER
+        time.sleep(1)
+
+        # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/
+        self.psm_to_battery_voltage = 11.0  # V/V
+        self.psm_to_battery_current_scale_factor = 37.8788  # A/V
+        self.psm_to_battery_current_offset = 0.330  # V   ##########DRIVER
+
+        # getting params in the ROS-config file (beluga.yaml)
+        self.critical_level = rospy.get_param("/battery/thresholds/critical",
+                                              default=13.5)
+        self.warning_level = rospy.get_param("/battery/thresholds/warning",
+                                             default=14.5)                   ###########NODE
+
+        # Polling intervals in seconds delay
+        system_interval = rospy.get_param("/battery/system/interval", 1)
+        logging_interval = rospy.get_param("/battery/logging/interval", 5)   ###########NODE
+
+        # Local variables
+        self.system_voltage = 0.0
+        self.system_current = 0.0   ##########DRIVER  -> WHY
+
+
+        self.system_voltage_state = (
+            "No receive"  # should only be "No receive", "Error", "Received"
+        )
+        self.system_current_state = (
+            "No receive"  # should only be "No receive", "Error try:
+
+            self.system_current = (self.channel_current.convert_and_read() -
+                                   self.psm_to_battery_current_offset
+                                   ) * self.psm_to_battery_current_scale_factor
+
+            if self.system_current_state != "Received":
+                self.system_current_state = "Received"
+
+        except IOError:
+            self.I2C_error_counter_current += 1
+            self.system_current_state = "Error"", "Received"
+        )
+
+        # Create ROS publishers
+        self.system_voltage_level_pub = rospy.Publisher(
+            "/auv/battery_level/system", Float32, queue_size=1)
+
+        self.system_current_level_pub = rospy.Publisher(
+            "/auv/current_level/system", Float32, queue_size=1)      ##########NODE
+
+
+        # set up callbacks
+        self.log_timer = rospy.Timer(
+            rospy.Duration(secs=logging_interval),
+            self.log_cb)  # for logging on ROS terminal      ##########NODE
+
+
+        self.system_timer = rospy.Timer(
+            rospy.Duration(secs=system_interval),
+            self.system_cb,  # will update and publish measurements to ROS  ##########ROS
+        )
+
+        rospy.loginfo("BatteryMonitor initialized")
+
+    def system_cb(self, event):     ###########NODE
+        self.read_PSM_voltage()
+        self.read_PSM_current()
+
+        self.system_voltage_level_pub.publish(self.system_voltage)
+        self.system_current_level_pub.publish(self.system_current)
+
+        if self.system_voltage < self.critical_level:
+            rospy.logerr(
+                f"Critical voltage: {self.system_voltage}V! Shutting down all active nodes!"
+            )
+            rospy.logerr(f"HAHA just kidding, let's blow up these batteries!")
+            # os.system("rosnode kill -a")
+
+    def log_cb(self, event):
+        if self.system_voltage_state == "Received":
+            self.log_voltage(self.system_voltage, "system")
+        if self.system_voltage_state == "Error":
+            rospy.logwarn(f"I2C Bus IOerror")
+        if self.system_voltage_state == "No receive":
+            rospy.loginfo("No voltage recieved from system yet.")
+
+        if self.system_current_state == "Received":
+            self.log_current(self.system_current, "system")
+        if self.system_current_state == "Error":
+            rospy.logwarn(f"I2C Bus IOerror")
+        if self.system_current_state == "No receive":
+            rospy.loginfo("No current recieved from system yet.")    #use switch in c++
+
+    def log_voltage(self, voltage, title):
+        if voltage == 0:
+            rospy.loginfo("Voltage is zero. Killswitch is probably off.")
+
+        elif voltage <= self.warning_level:
+            rospy.logwarn("%s voltage: %.3f V" % (title, voltage))
+
+        else:
+            rospy.loginfo("%s voltage: %.3f V" % (title, voltage))
+
+    def log_current(self, current, title):
+        if current <= 0:
+            rospy.loginfo("No current draw...")
+        else:
+            rospy.loginfo("%s current: %.3f A" % (title, current))
+
+    def read_PSM_voltage(self):
+        # Sometimes an I/O timeout or error happens, it will run again when the error disappears
+        try:  #what is the function convert_and_read?
+            self.system_voltage = (self.channel_voltage.convert_and_read() *
+                                   self.psm_to_battery_voltage)
+
+            if self.system_voltage_state != "Received":
+                self.system_voltage_state = "Received"
+
+        except IOError:
+            self.I2C_error_counter_voltage += 1
+            self.system_voltage_state = "Error"
+            rospy.logwarn(f"I2C Bus IOerror")  # for debug
+
+    def read_PSM_current(self):
+        try:
+            self.system_current = (self.channel_current.convert_and_read() -  #from where comes this function -> MCP342x?
+                                   self.psm_to_battery_current_offset
+                                   ) * self.psm_to_battery_current_scale_factor
+            #I don't from where the formula comes 
+
+            if self.system_current_state != "Received":
+                self.system_current_state = "Received"
+
+        except IOError:
+            self.I2C_error_counter_current += 1 #this variable isn't initialized?
+            self.system_current_state = "Error"
+            # rospy.logwarn(f"I2C Bus IOerror. Voltage error counter : {self.I2C_error_counter_current}")
+
+    def shutdown(self):
+        self.system_timer.shutdown()
+        self.log_timer.shutdown()
+        self.bus.close()
+
+
+if __name__ == "__main__":
+    bm = BatteryMonitor()
+    try:
+        rospy.spin()
+    finally:
+        bm.shutdown()

mission/internal_status/src/battery_monitor_lib.cpp

@@ -0,0 +1,128 @@
+#include <iostream>
+using namespace std;     //do i need this?
+
+#include <time.h>
+#include "i2c/smbus.h"
+//#include <MCP342x.h>
+
+// include doesn't work well
+
+//how to know the types of the values?
+
+//
+
+/*
+class BatteryMonitor{
+
+public:
+    BatteryMonitor()  //default constructor to initialize the class
+    {  
+        //***************I2C protocole*******************************
+        i2c_adress = 0x69;
+        bus = smbus.SMBus(1);
+        channel_voltage = MCP342x(bus,i2c_adress,channel=0,resolution=18);
+        channel_current = MCP342x(bus,i2c_adress,channel=1,resolution=18);
+        //TIME SLEEP? -> only for rospy?
+        
+        //**********convertion ratio taken from PSM datasheet**************
+        psm_to_battery_voltage = 11.0;
+        psm_to_battery_current_scale_factor =37.8788;
+        psm_to_battery_current_offset = 0.330;
+
+
+        system_voltage = 0.0;
+        system_current = 0.0;
+
+        system_voltage_state = "No receive";
+        system_current_state = "No receive";
+
+        I2C_error_counter_current = 0;
+        I2C_error_counter_voltage = 0;
+
+        cout << "Iniatialization done" << endl;
+    }
+
+    float get_PSM_current(){
+
+        try {
+            system_current = ((channel_current.convert_and_read() 
+                            - psm_to_battery_current_offset) 
+                            * psm_to_battery_current_scale_factor);
+
+            if (system_current_state != "Received") {
+                system_current_state = "Received";
+            }
+        }
+
+        catch (const std::ios_base::failure& e) {
+            I2C_error_counter_current += 1;
+            system_current_state = "Error";
+            cerr << "Error: " << e.what() << endl;
+        }
+        
+        return system_current;
+    }
+
+    float get_PSM_voltage(){
+
+        try {
+            system_voltage = (channel_voltage.convert_and_read()
+                            * psm_to_battery_voltage);
+            
+            if (system_voltage_state != "Received") {
+                system_voltage_state = "Received";
+            }
+        }
+
+        catch (const std::ios_base::failure& e) {
+            I2C_error_counter_voltage += 1;
+            system_current_state = "Error";
+            cerr << "Error: " << e.what() << endl;
+        }
+
+        return system_voltage;
+    }
+
+    void shutdown(){
+
+        bus.close();
+    }
+
+private:
+    int16_t i2c_adress;
+    int bus;
+    float channel_voltage; //what's the type of these value?
+    float channel_current;  //what's the type of these value?
+    float psm_to_battery_voltage;
+    float psm_to_battery_current_scale_factor;
+    float psm_to_battery_current_offset;
+    float system_voltage;
+    float system_current;
+    string system_voltage_state;
+    string system_current_state;
+    int I2C_error_counter_current;
+    int I2C_error_counter_voltage;
+
+};
+
+*/
+
+/* TRY CATCH BLOCK
+
+
+try {
+    system_current = (channel_current.convert_and_read() 
+                    - psm_to_battery_current_offset) 
+                    * psm_to_battery_current_scale_factor;
+
+    if (system_current_state != "Received") {
+        system_current_state = "Received";
+    }
+}
+catch (const std::ios_base::failure& e) {
+    I2C_error_counter_current += 1;
+    system_current_state = "Error";
+}
+
+
+*/