update docs, update config, tweaks to node

README.md

@@ -30,11 +30,12 @@ GPS related Datasheets
 ## ROS2 Package Info
 
 ### Dependencies
-* RTIMULib (c++ and python)
+* RTIMULib (c++ and python) found [here](https://github.com/mattanimation/RTIMULib2)
 
 ## Setup
 for now, RTIMULib should be installed via the instructions of the readme located: `Linux/python`
 **note:**: python3 should be used
+this is required due to the fact that this repo has added the BMP280 that the v3 of the BerryGPS has.
 
 * change any setttings in the `config.yaml` and if need be
 * the `RTIMULib.ini` file should be fine minus the need to calibrate the magnitometer

config/RTIMULib.ini

@@ -64,6 +64,14 @@ AxisRotation=0
 #   6 = MS5637
 PressureType=3
 
+#I2C pressure rate register / int value (default is 1)
+#  0 = skip
+#  1 = ultra low power
+#  2 = lop power
+#  3 = standard
+#  4 = high power
+#  5 = ultra high power
+PressureRate=4
 # 
 # I2C pressure sensor address (filled in automatically by auto discover) 
 I2CPressureAddress=119

ros2_berrygps/imu_node.py

@@ -11,7 +11,7 @@
 from sensor_msgs.msg import Imu
 from geometry_msgs.msg import Quaternion, Vector3
 
-from ros2_berrygps.imu_utils import compute_height, G_TO_MPSS
+from ros2_berrygps.imu_utils import get_altitude, G_TO_MPSS
 from ros2_berrygps import better_get_parameter_or
 
 
@@ -120,22 +120,24 @@ def handle_imu(self):
             if data is not None:
                 # self.get_logger().info("IMU_DATA: {0}".format(data))
                 (data["pressureValid"], data["pressure"], data["temperatureValid"], data["temperature"]) = self.pressure.pressureRead()
-                fusionPose = data["fusionPose"]
+                #fusionPose = data["fusionPose"]
                 #self.get_logger().info("r: {0} p: {1} y: {2}".format(
                 #    math.degrees(fusionPose[0]), 
                 #    math.degrees(fusionPose[1]),
                 #    math.degrees(fusionPose[2])
                 #))
 
                 # TODO : publish this info too for GPS fusion
-                #if data["pressureValid"]:
-                #    self.get_logger().info("Pressure: {0}, height above sea level: {1}".format(
-                #        data["pressure"],
-                #        compute_height(data["pressure"])
-                #    ))
+                # TODO: the get_altitude should get local pressure in hPa
+                # example: inhg of 30.1 to hpa was 1019.30
+                if data["pressureValid"]:
+                    self.get_logger().info("Pressure: {0}, height above sea level: {1}".format(
+                        data["pressure"],
+                        get_altitude(data["pressure"], sea_level_hPa=1019.30)
+                    ))
 
-                #if data["temperatureValid"]:
-                #    self.get_logger().info("Temperature: {0}".format(data["temperature"]))
+                if data["temperatureValid"]:
+                    self.get_logger().info("Temperature C: {0} Temperature F: {1}".format(data["temperature"], (data["temperature"] * (9/5)) + 32 ))
 
                 # build msg and publish
                 qfp = data['fusionQPose']

ros2_berrygps/imu_utils.py

@@ -2,8 +2,7 @@
 G_TO_MPSS = 9.80665
 
 
-
-def compute_height(pressure: float) -> float:
+def get_altitude(pressure: float, sea_level_hPa: float = 1013.25) -> float:
     """
     the conversion uses the formula:
     
@@ -21,9 +20,26 @@ def compute_height(pressure: float) -> float:
     h = 44330.8 * (1 - (p / P0)**0.190263)
     
     Arguments:
-        pressure {float} -- [description]
+        pressure {float} -- current pressure 
+        sea_level_hPa {float} -- The current hPa at sea level.
     
     Returns:
         [type] -- [description]
     """
-    return 44330.8 * (1 - pow(pressure / 1013.25, 0.190263))
+    return 44330.8 * (1 - pow(pressure / sea_level_hPa, 0.190263))
+
+def compute_sea_level(altitude: float, atmospheric: float) -> float:
+    """
+    Calculates the pressure at sea level (in hPa) from the specified altitude
+    (in meters), and atmospheric pressure (in hPa).
+    # Equation taken from BMP180 datasheet (page 17):
+    # http://www.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf
+
+    Args:
+        altitude    :  Altitude in meters
+        atmospheric :  Atmospheric pressure in hPa
+    
+    Return:
+        float The approximate pressure
+    """
+    return atmospheric / pow(1.0 - (altitude / 44330.0), 5.255)