diff --git a/onboard/physical/drivers/MS5837.py b/onboard/physical/drivers/MS5837.py new file mode 100644 index 0000000..cab71f9 --- /dev/null +++ b/onboard/physical/drivers/MS5837.py @@ -0,0 +1,80 @@ +# The MIT License (MIT) +# Copyright (c) 2017 Blue Robotics + +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# Edited from example code https://github.com/bluerobotics/ms5837-python/blob/master/example.py + +import ms5837 +import time + + +# TODO: Change once we know what port on ROV +#sensor = ms5837.MS5837_30BA() # Default I2C bus is 1 (Raspberry Pi 3) +#sensor = ms5837.MS5837_30BA(0) # Specify I2C bus +sensor = ms5837.MS5837_02BA() +#sensor = ms5837.MS5837_02BA(0) +#sensor = ms5837.MS5837(model=ms5837.MS5837_MODEL_30BA, bus=0) # Specify model and bus + +# We must initialize the sensor before reading it +if sensor.init(): + print("Sensor initialized") +else : + print("Sensor could not be initialized") + exit(1) + +# We have to read values from sensor to update pressure and temperature +if not sensor.read(): + print("Sensor read failed!") + exit(1) + +print(("Pressure: %.2f atm %.2f Torr %.2f psi") % ( +sensor.pressure(ms5837.UNITS_atm), +sensor.pressure(ms5837.UNITS_Torr), +sensor.pressure(ms5837.UNITS_psi))) + +print(("Temperature: %.2f C %.2f F %.2f K") % ( +sensor.temperature(ms5837.UNITS_Centigrade), +sensor.temperature(ms5837.UNITS_Farenheit), +sensor.temperature(ms5837.UNITS_Kelvin))) + +freshwaterDepth = sensor.depth() # default is freshwater +sensor.setFluidDensity(ms5837.DENSITY_SALTWATER) +saltwaterDepth = sensor.depth() # No need to read() again +# sensor.setFluidDensity(1000) # kg/m^3 TODO: staying with saltwater. Will change to make it default later +print(("Depth: %.3f m (freshwater) %.3f m (saltwater)") % (freshwaterDepth, saltwaterDepth)) + +# fluidDensity doesn't matter for altitude() (always MSL air density) +print(("MSL Relative Altitude: %.2f m") % sensor.altitude()) # relative to Mean Sea Level pressure in air + +time.sleep(5) + +# Spew readings +# Just printing for initial sensor testing, next step is to get on websocket to surface +while True: + if sensor.read(): + print(("Depth: %0.03f m P: %0.1f mbar %0.3f psi\tT: %0.2f C %0.2f F") % ( + sensor.depth(), # saltwater depth + sensor.pressure(), # Default is mbar (no arguments) + sensor.pressure(ms5837.UNITS_psi), # Request psi + sensor.temperature(), # Default is degrees C (no arguments) + sensor.temperature(ms5837.UNITS_Farenheit))) # Request Farenheit + else: + print("Sensor read failed!") + exit(1) diff --git a/onboard/physical/drivers/ms5837/__init__.py b/onboard/physical/drivers/ms5837/__init__.py new file mode 100644 index 0000000..8b4d56b --- /dev/null +++ b/onboard/physical/drivers/ms5837/__init__.py @@ -0,0 +1 @@ +from .ms5837 import * diff --git a/onboard/physical/drivers/ms5837/ms5837.py b/onboard/physical/drivers/ms5837/ms5837.py new file mode 100644 index 0000000..365094a --- /dev/null +++ b/onboard/physical/drivers/ms5837/ms5837.py @@ -0,0 +1,259 @@ +try: + import smbus2 as smbus +except: + print('Try sudo apt-get install python-smbus2') + +from time import sleep + +# Models +MODEL_02BA = 0 +MODEL_30BA = 1 +MODEL_UNKNOWN = 255 + +# context: https://github.com/ArduPilot/ardupilot/pull/29122#issuecomment-2877269114 +MS5837_02BA_MAX_SENSITIVITY = 49000; +MS5837_02BA_30BA_SEPARATION = 37000; +MS5837_30BA_MIN_SENSITIVITY = 26000; + +# Oversampling options +OSR_256 = 0 +OSR_512 = 1 +OSR_1024 = 2 +OSR_2048 = 3 +OSR_4096 = 4 +OSR_8192 = 5 + +# kg/m^3 convenience +DENSITY_FRESHWATER = 997 +DENSITY_SALTWATER = 1029 + +# Conversion factors (from native unit, mbar) +UNITS_Pa = 100.0 +UNITS_hPa = 1.0 +UNITS_kPa = 0.1 +UNITS_mbar = 1.0 +UNITS_bar = 0.001 +UNITS_atm = 0.000986923 +UNITS_Torr = 0.750062 +UNITS_psi = 0.014503773773022 + +# Valid units +UNITS_Centigrade = 1 +UNITS_Farenheit = 2 +UNITS_Kelvin = 3 + + +class MS5837(object): + + # Registers + _MS5837_ADDR = 0x76 + _MS5837_RESET = 0x1E + _MS5837_ADC_READ = 0x00 + _MS5837_PROM_READ = 0xA0 + _MS5837_CONVERT_D1_256 = 0x40 + _MS5837_CONVERT_D2_256 = 0x50 + + def __init__(self, model=MODEL_UNKNOWN, bus=1): + self._model = model + + try: + self._bus = smbus.SMBus(bus) + except: + print("Bus %d is not available."%bus) + print("Available busses are listed as /dev/i2c*") + self._bus = None + + self._fluidDensity = DENSITY_FRESHWATER + self._pressure = 0 + self._temperature = 0 + self._D1 = 0 + self._D2 = 0 + + def init(self): + if self._bus is None: + "No bus!" + return False + + self._bus.write_byte(self._MS5837_ADDR, self._MS5837_RESET) + + # Wait for reset to complete + sleep(0.01) + + self._C = [] + + # Read calibration values and CRC + for i in range(7): + c = self._bus.read_word_data(self._MS5837_ADDR, self._MS5837_PROM_READ + 2*i) + c = ((c & 0xFF) << 8) | (c >> 8) # SMBus is little-endian for word transfers, we need to swap MSB and LSB + self._C.append(c) + + crc = (self._C[0] & 0xF000) >> 12 + if crc != self._crc4(self._C): + print("PROM read error, CRC failed!") + return False + + if self._model == MODEL_UNKNOWN: + self.auto_detect_model() + + return True + + def auto_detect_model(self): + """ Automatically detect sensor variant, with a heuristic threshold. + + Details in https://github.com/ArduPilot/ardupilot/pull/29122#issuecomment-2877269114x + + TODO: include detection of product version / package type, from PROM Word 0, per + https://github.com/ArduPilot/ardupilot/pull/29122#pullrequestreview-2837597764 + """ + pressure_sensitivity = self._C[1] + if MS5837_30BA_MIN_SENSITIVITY <= pressure_sensitivity <= MS5837_02BA_30BA_SEPARATION: + self._model = MODEL_30BA + elif MS5837_02BA_30BA_SEPARATION < pressure_sensitivity <= MS5837_02BA_MAX_SENSITIVITY: + self._model = MODEL_02BA + else: + self._model = MODEL_UNKNOWN + + def read(self, oversampling=OSR_8192): + if self._bus is None: + print("No bus!") + return False + + if oversampling < OSR_256 or oversampling > OSR_8192: + print("Invalid oversampling option!") + return False + + # Request D1 conversion (pressure) + self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D1_256 + 2*oversampling) + + # Maximum conversion time increases linearly with oversampling + # max time (seconds) ~= 2.2e-6(x) where x = OSR = (2^8, 2^9, ..., 2^13) + # We use 2.5e-6 for some overhead + sleep(2.5e-6 * 2**(8+oversampling)) + + d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) + self._D1 = d[0] << 16 | d[1] << 8 | d[2] + + # Request D2 conversion (temperature) + self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D2_256 + 2*oversampling) + + # As above + sleep(2.5e-6 * 2**(8+oversampling)) + + d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) + self._D2 = d[0] << 16 | d[1] << 8 | d[2] + + # Calculate compensated pressure and temperature + # using raw ADC values and internal calibration + self._calculate() + + return True + + def setFluidDensity(self, denisty): + self._fluidDensity = denisty + + # Pressure in requested units + # mbar * conversion + def pressure(self, conversion=UNITS_mbar): + return self._pressure * conversion + + # Temperature in requested units + # default degrees C + def temperature(self, conversion=UNITS_Centigrade): + degC = self._temperature / 100.0 + if conversion == UNITS_Farenheit: + return (9.0/5.0)*degC + 32 + elif conversion == UNITS_Kelvin: + return degC + 273 + return degC + + # Depth relative to MSL pressure in given fluid density + def depth(self): + return (self.pressure(UNITS_Pa)-101300)/(self._fluidDensity*9.80665) + + # Altitude relative to MSL pressure + def altitude(self): + return (1-pow((self.pressure()/1013.25),.190284))*145366.45*.3048 + + # Cribbed from datasheet + def _calculate(self): + OFFi = 0 + SENSi = 0 + Ti = 0 + + dT = self._D2-self._C[5]*256 + if self._model == MODEL_02BA: + SENS = self._C[1]*65536+(self._C[3]*dT)/128 + OFF = self._C[2]*131072+(self._C[4]*dT)/64 + self._pressure = (self._D1*SENS/(2097152)-OFF)/(32768) + elif self._model == MODEL_30BA: + SENS = self._C[1]*32768+(self._C[3]*dT)/256 + OFF = self._C[2]*65536+(self._C[4]*dT)/128 + self._pressure = (self._D1*SENS/(2097152)-OFF)/(8192) + else: + raise NotImplementedError("Cannot calculate pressure for unknown model type!") + + self._temperature = 2000+dT*self._C[6]/8388608 + + # Second order compensation + if self._model == MODEL_02BA: + if (self._temperature/100) < 20: # Low temp + Ti = (11*dT*dT)/(34359738368) + OFFi = (31*(self._temperature-2000)*(self._temperature-2000))/8 + SENSi = (63*(self._temperature-2000)*(self._temperature-2000))/32 + + else: + if (self._temperature/100) < 20: # Low temp + Ti = (3*dT*dT)/(8589934592) + OFFi = (3*(self._temperature-2000)*(self._temperature-2000))/2 + SENSi = (5*(self._temperature-2000)*(self._temperature-2000))/8 + if (self._temperature/100) < -15: # Very low temp + OFFi = OFFi+7*(self._temperature+1500)*(self._temperature+1500) + SENSi = SENSi+4*(self._temperature+1500)*(self._temperature+1500) + elif (self._temperature/100) >= 20: # High temp + Ti = 2*(dT*dT)/(137438953472) + OFFi = (1*(self._temperature-2000)*(self._temperature-2000))/16 + SENSi = 0 + + OFF2 = OFF-OFFi + SENS2 = SENS-SENSi + + if self._model == MODEL_02BA: + self._temperature = (self._temperature-Ti) + self._pressure = (((self._D1*SENS2)/2097152-OFF2)/32768)/100.0 + else: + self._temperature = (self._temperature-Ti) + self._pressure = (((self._D1*SENS2)/2097152-OFF2)/8192)/10.0 + + # Cribbed from datasheet + def _crc4(self, n_prom): + n_rem = 0 + + n_prom[0] = ((n_prom[0]) & 0x0FFF) + n_prom.append(0) + + for i in range(16): + if i%2 == 1: + n_rem ^= ((n_prom[i>>1]) & 0x00FF) + else: + n_rem ^= (n_prom[i>>1] >> 8) + + for n_bit in range(8,0,-1): + if n_rem & 0x8000: + n_rem = (n_rem << 1) ^ 0x3000 + else: + n_rem = (n_rem << 1) + + n_rem = ((n_rem >> 12) & 0x000F) + + self.n_prom = n_prom + self.n_rem = n_rem + + return n_rem ^ 0x00 + +class MS5837_30BA(MS5837): + def __init__(self, bus=1): + MS5837.__init__(self, MODEL_30BA, bus) + +class MS5837_02BA(MS5837): + def __init__(self, bus=1): + MS5837.__init__(self, MODEL_02BA, bus) diff --git a/onboard/physical/physical.py b/onboard/physical/physical.py index 0af8b68..7cc908d 100644 --- a/onboard/physical/physical.py +++ b/onboard/physical/physical.py @@ -1,21 +1,22 @@ -import time import busio import board import time from gpiozero import Servo +import ms5837 class ROV: def __init__(self): self.servos = {} try: i2c = busio.I2C(board.SCL, board.SDA) + self.bar02 = ms5837.MS5837_02BA() except Exception as e: print(e) print("UNABLE TO CONNECT TO IMU") # number = GPIO number # value = PWM value - def set_pin_pwm(self, number: int, value: int): + def set_pin_pwm(self, number: int, value: int): # print(number, value) if number not in self.servos: self.servos[number] = Servo(number) @@ -27,9 +28,23 @@ def set_pin_pwm(self, number: int, value: int): async def flush_pin_pwms(self): pass + # get depth function, heavily based on quinns imu function + def get_depth(self) -> dict: + if self.bar02 is None: + return {} + try: + depth = self.bar02.depth + if depth[0] is None: + return {} + return {"depth": depth} + except: + return {} + async def poll_sensors(self) -> dict: readings = [] + readings.append(self.get_depth) + # Will be a list of (maybe empty) dictionaries of readings to report readings_dict = {} for reading in readings: