diff --git a/Python/ADC.py b/Python/ADC.py index e2dee46a..e8aec84f 100644 --- a/Python/ADC.py +++ b/Python/ADC.py @@ -1,17 +1,30 @@ -import sys, time +import sys, signal, time +import navio.Common.util -import navio.adc -import navio.util -navio.util.check_apm() +if navio.Common.util.get_navio_version() == "NAVIO2": + import navio.Navio2.ADC as ADC +else: + import navio.Navio.ADC as ADC -adc = navio.adc.ADC() +navio.Common.util.check_apm() + + +def signal_handler(signal, frame): + print "You pressed Ctrl+C!" + sys.exit(0) + + +signal.signal(signal.SIGINT, signal_handler) +# print 'Press Ctrl+C to exit' + +adc = ADC() results = [0] * adc.channel_count -while (True): - s = '' - for i in range (0, adc.channel_count): +while True: + s = "" + for i in range(0, adc.channel_count): results[i] = adc.read(i) - s += 'A{0}: {1:6.4f}V '.format(i, results[i] / 1000) - print(s) + s += "A{0}: {1:6.4f}V ".format(i, results[i] / 1000) + print (s) time.sleep(0.5) diff --git a/Python/AccelGyroMag.py b/Python/AccelGyroMag.py index 41c7d226..374cf6ca 100644 --- a/Python/AccelGyroMag.py +++ b/Python/AccelGyroMag.py @@ -26,38 +26,51 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. """ -import spidev import time -import argparse +import argparse import sys -import navio.mpu9250 -import navio.util -navio.util.check_apm() +import navio.Common.mpu9250 +import navio.Common.util +import navio.Navio2.lsm9ds1 -parser = argparse.ArgumentParser() -parser.add_argument("-i", help = "Sensor selection: -i [sensor name]. Sensors names: mpu is MPU9250, lsm is LSM9DS1") +navio.Common.util.check_apm() -if len(sys.argv) == 1: - print "Enter parameter" - parser.print_help() - sys.exit(1) -elif len(sys.argv) == 2: - sys.exit("Enter sensor name: mpu or lsm") -args = parser.parse_args() +def get_inertial_sensor(): -if args.i == 'mpu': - print "Selected: MPU9250" - imu = navio.mpu9250.MPU9250() -elif args.i == 'lsm': - print "Selected: LSM9DS1" - imu = navio.lsm9ds1.LSM9DS1() -else: - print "Wrong sensor name. Select: mpu or lsm" - sys.exit(1) + if navio.Common.util.get_navio_version() == "NAVIO2": + + parser = argparse.ArgumentParser() + parser.add_argument( + "-i", + help="Sensor selection: -i [sensor name]. Sensors names: mpu is MPU9250, lsm is LSM9DS1", + ) + + if len(sys.argv) == 1: + print "Enter parameter" + parser.print_help() + sys.exit(1) + elif len(sys.argv) == 2: + sys.exit("Enter sensor name: mpu or lsm") + + args = parser.parse_args() + + if args.i == "mpu": + print "Selected: MPU9250" + return navio.Common.mpu9250.MPU9250() + elif args.i == "lsm": + print "Selected: LSM9DS1" + return navio.Navio2.lsm9ds1.LSM9DS1() + else: + print "Wrong sensor name. Select: mpu or lsm" + sys.exit(1) + else: + print "Selected: MPU9250" + return navio.Common.mpu9250.MPU9250() +imu = get_inertial_sensor() if imu.testConnection(): print "Connection established: True" @@ -69,23 +82,29 @@ time.sleep(1) while True: - # imu.read_all() - # imu.read_gyro() - # imu.read_acc() - # imu.read_temp() - # imu.read_mag() - - # print "Accelerometer: ", imu.accelerometer_data - # print "Gyroscope: ", imu.gyroscope_data - # print "Temperature: ", imu.temperature - # print "Magnetometer: ", imu.magnetometer_data - - # time.sleep(0.1) - - m9a, m9g, m9m = imu.getMotion9() - - print "Acc:", "{:+7.3f}".format(m9a[0]), "{:+7.3f}".format(m9a[1]), "{:+7.3f}".format(m9a[2]), - print " Gyr:", "{:+8.3f}".format(m9g[0]), "{:+8.3f}".format(m9g[1]), "{:+8.3f}".format(m9g[2]), - print " Mag:", "{:+7.3f}".format(m9m[0]), "{:+7.3f}".format(m9m[1]), "{:+7.3f}".format(m9m[2]) - - time.sleep(0.5) + # imu.read_all() + # imu.read_gyro() + # imu.read_acc() + # imu.read_temp() + # imu.read_mag() + + # print "Accelerometer: ", imu.accelerometer_data + # print "Gyroscope: ", imu.gyroscope_data + # print "Temperature: ", imu.temperature + # print "Magnetometer: ", imu.magnetometer_data + + # time.sleep(0.1) + + m9a, m9g, m9m = imu.getMotion9() + + print "Acc:", "{:+7.3f}".format(m9a[0]), "{:+7.3f}".format( + m9a[1] + ), "{:+7.3f}".format(m9a[2]), + print " Gyr:", "{:+8.3f}".format(m9g[0]), "{:+8.3f}".format( + m9g[1] + ), "{:+8.3f}".format(m9g[2]), + print " Mag:", "{:+7.3f}".format(m9m[0]), "{:+7.3f}".format( + m9m[1] + ), "{:+7.3f}".format(m9m[2]) + + time.sleep(0.5) diff --git a/Python/Barometer.py b/Python/Barometer.py index 6e9480e0..5948cf33 100644 --- a/Python/Barometer.py +++ b/Python/Barometer.py @@ -10,25 +10,26 @@ import time -import navio.ms5611 -import navio.util +import navio.Common.util -navio.util.check_apm() +import navio.Common.ms5611 -baro = navio.ms5611.MS5611() +navio.Common.util.check_apm() + +baro = navio.Common.ms5611.MS5611() baro.initialize() -while(True): - baro.refreshPressure() - time.sleep(0.01) # Waiting for pressure data ready 10ms - baro.readPressure() +while True: + baro.refreshPressure() + time.sleep(0.01) # Waiting for pressure data ready 10ms + baro.readPressure() - baro.refreshTemperature() - time.sleep(0.01) # Waiting for temperature data ready 10ms - baro.readTemperature() + baro.refreshTemperature() + time.sleep(0.01) # Waiting for temperature data ready 10ms + baro.readTemperature() - baro.calculatePressureAndTemperature() + baro.calculatePressureAndTemperature() - print "Temperature(C): %.6f" % (baro.TEMP), "Pressure(millibar): %.6f" % (baro.PRES) + print "Temperature(C): %.6f" % (baro.TEMP), "Pressure(millibar): %.6f" % (baro.PRES) - time.sleep(1) + time.sleep(1) diff --git a/Python/GPS.py b/Python/GPS.py index d3b7a1f3..d9099971 100644 --- a/Python/GPS.py +++ b/Python/GPS.py @@ -1,54 +1,82 @@ -import navio.util -import navio.ublox - +import navio.Common.ublox if __name__ == "__main__": - ubl = navio.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2) + ubl = navio.Common.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2) ubl.configure_poll_port() - ubl.configure_poll(navio.ublox.CLASS_CFG, navio.ublox.MSG_CFG_USB) - #ubl.configure_poll(navio.ublox.CLASS_MON, navio.ublox.MSG_MON_HW) + ubl.configure_poll(navio.Common.ublox.CLASS_CFG, navio.Common.ublox.MSG_CFG_USB) + # ubl.configure_poll(navio.ublox.CLASS_MON, navio.ublox.MSG_MON_HW) - ubl.configure_port(port=navio.ublox.PORT_SERIAL1, inMask=1, outMask=0) - ubl.configure_port(port=navio.ublox.PORT_USB, inMask=1, outMask=1) - ubl.configure_port(port=navio.ublox.PORT_SERIAL2, inMask=1, outMask=0) + ubl.configure_port(port=navio.Common.ublox.PORT_SERIAL1, inMask=1, outMask=0) + ubl.configure_port(port=navio.Common.ublox.PORT_USB, inMask=1, outMask=1) + ubl.configure_port(port=navio.Common.ublox.PORT_SERIAL2, inMask=1, outMask=0) ubl.configure_poll_port() - ubl.configure_poll_port(navio.ublox.PORT_SERIAL1) - ubl.configure_poll_port(navio.ublox.PORT_SERIAL2) - ubl.configure_poll_port(navio.ublox.PORT_USB) + ubl.configure_poll_port(navio.Common.ublox.PORT_SERIAL1) + ubl.configure_poll_port(navio.Common.ublox.PORT_SERIAL2) + ubl.configure_poll_port(navio.Common.ublox.PORT_USB) ubl.configure_solution_rate(rate_ms=1000) ubl.set_preferred_dynamic_model(None) ubl.set_preferred_usePPP(None) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSLLH, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_PVT, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_STATUS, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_SOL, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELNED, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_SVINFO, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_VELECEF, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_POSECEF, 1) - ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_RAW, 1) - ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SFRB, 1) - ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_SVSI, 1) - ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_ALM, 1) - ubl.configure_message_rate(navio.ublox.CLASS_RXM, navio.ublox.MSG_RXM_EPH, 1) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_TIMEGPS, 5) - ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_CLOCK, 5) - #ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_DGPS, 5) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_POSLLH, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_PVT, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_STATUS, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_SOL, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_VELNED, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_SVINFO, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_VELECEF, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_POSECEF, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_RXM, navio.Common.ublox.MSG_RXM_RAW, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_RXM, navio.Common.ublox.MSG_RXM_SFRB, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_RXM, navio.Common.ublox.MSG_RXM_SVSI, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_RXM, navio.Common.ublox.MSG_RXM_ALM, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_RXM, navio.Common.ublox.MSG_RXM_EPH, 1 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_TIMEGPS, 5 + ) + ubl.configure_message_rate( + navio.Common.ublox.CLASS_NAV, navio.Common.ublox.MSG_NAV_CLOCK, 5 + ) + # ubl.configure_message_rate(navio.ublox.CLASS_NAV, navio.ublox.MSG_NAV_DGPS, 5) while True: msg = ubl.receive_message() if msg is None: if opts.reopen: ubl.close() - ubl = navio.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2) + ubl = navio.Common.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2) continue print(empty) break - #print(msg.name()) + # print(msg.name()) if msg.name() == "NAV_POSLLH": outstr = str(msg).split(",")[1:] outstr = "".join(outstr) @@ -57,4 +85,4 @@ outstr = str(msg).split(",")[1:2] outstr = "".join(outstr) print(outstr) - #print(str(msg)) + # print(str(msg)) diff --git a/Python/LED.py b/Python/LED.py index 1966a3a4..b0011080 100644 --- a/Python/LED.py +++ b/Python/LED.py @@ -1,39 +1,48 @@ -import sys - -import navio.leds import time -import navio.util +import os + +import navio.Common.util + +if navio.Common.util.get_navio_version() == "NAVIO2": + import navio.Navio2.Led as Led +else: + import navio.Navio.Led as Led + + +if os.getuid() != 0: + print "Not root. Please, launch like this: sudo python LED.py" + exit(-1) -navio.util.check_apm() +navio.Common.util.check_apm() -led = navio.leds.Led() +led = Led() -led.setColor('Yellow') +led.setColor("Yellow") print "LED is yellow" time.sleep(1) -while (True): +while True: - led.setColor('Green') + led.setColor("Green") print "LED is green" time.sleep(1) - led.setColor('Cyan') + led.setColor("Cyan") print "LED is cyan" time.sleep(1) - led.setColor('Blue') + led.setColor("Blue") print "LED is blue" time.sleep(1) - led.setColor('Magenta') + led.setColor("Magenta") print "LED is magenta" time.sleep(1) - led.setColor('Red') + led.setColor("Red") print "LED is red" time.sleep(1) - led.setColor('Yellow') + led.setColor("Yellow") print "LED is yellow" time.sleep(1) diff --git a/Python/RCInput.py b/Python/RCInput.py index 547a031e..d0ccce8f 100644 --- a/Python/RCInput.py +++ b/Python/RCInput.py @@ -1,13 +1,16 @@ -import sys, time +import time -import navio.rcinput -import navio.util +import navio.Common.util -navio.util.check_apm() +if navio.Common.util.get_navio_version() == "NAVIO2": + import navio.Navio2.RCInput as RCInput +else: + import navio.Navio.RCInput as RCInput -rcin = navio.rcinput.RCInput() +navio.Common.util.check_apm() -while (True): +rcin = RCInput() +while True: period = rcin.read(2) print period time.sleep(1) diff --git a/Python/Servo.py b/Python/Servo.py index 2e30e923..f7a719ce 100644 --- a/Python/Servo.py +++ b/Python/Servo.py @@ -1,20 +1,28 @@ -import sys import time +import os -import navio.pwm -import navio.util +import navio.Common.util -navio.util.check_apm() +if navio.Common.util.get_navio_version() == "NAVIO2": + import navio.Navio2.RCOutput as RCOutput +else: + import navio.Navio.RCOutput as RCOutput +SERVO_MIN = 1.250 # ms +SERVO_MAX = 1.750 # ms PWM_OUTPUT = 0 -SERVO_MIN = 1.250 #ms -SERVO_MAX = 1.750 #ms -with navio.pwm.PWM(PWM_OUTPUT) as pwm: +if os.getuid() != 0: + print "Not root. Please, launch like this: sudo python Servo.py" + exit(-1) + +navio.Common.util.check_apm() + +with RCOutput(PWM_OUTPUT) as pwm: pwm.set_period(50) pwm.enable() - while (True): + while True: pwm.set_duty_cycle(SERVO_MIN) time.sleep(1) pwm.set_duty_cycle(SERVO_MAX) diff --git a/Python/navio/Common/__init__.py b/Python/navio/Common/__init__.py new file mode 100644 index 00000000..374cdc4e --- /dev/null +++ b/Python/navio/Common/__init__.py @@ -0,0 +1,2 @@ +from .mpu9250 import MPU9250 +from .ms5611 import MS5611 diff --git a/Python/navio/Common/mpu9250.py b/Python/navio/Common/mpu9250.py new file mode 100644 index 00000000..9e9a3b9b --- /dev/null +++ b/Python/navio/Common/mpu9250.py @@ -0,0 +1,628 @@ +""" +MPU9250 driver code is placed under the BSD license. +Copyright (c) 2014, Emlid Limited, www.emlid.com +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Emlid Limited nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +""" + +import spidev +import time +import struct +import array + + +class MPU9250: + + G_SI = 9.80665 + PI = 3.14159 + + # MPU9250 registers + __MPUREG_XG_OFFS_TC = 0x00 + __MPUREG_YG_OFFS_TC = 0x01 + __MPUREG_ZG_OFFS_TC = 0x02 + __MPUREG_X_FINE_GAIN = 0x03 + __MPUREG_Y_FINE_GAIN = 0x04 + __MPUREG_Z_FINE_GAIN = 0x05 + __MPUREG_XA_OFFS_H = 0x06 + __MPUREG_XA_OFFS_L = 0x07 + __MPUREG_YA_OFFS_H = 0x08 + __MPUREG_YA_OFFS_L = 0x09 + __MPUREG_ZA_OFFS_H = 0x0A + __MPUREG_ZA_OFFS_L = 0x0B + __MPUREG_PRODUCT_ID = 0x0C + __MPUREG_SELF_TEST_X = 0x0D + __MPUREG_SELF_TEST_Y = 0x0E + __MPUREG_SELF_TEST_Z = 0x0F + __MPUREG_SELF_TEST_A = 0x10 + __MPUREG_XG_OFFS_USRH = 0x13 + __MPUREG_XG_OFFS_USRL = 0x14 + __MPUREG_YG_OFFS_USRH = 0x15 + __MPUREG_YG_OFFS_USRL = 0x16 + __MPUREG_ZG_OFFS_USRH = 0x17 + __MPUREG_ZG_OFFS_USRL = 0x18 + __MPUREG_SMPLRT_DIV = 0x19 + __MPUREG_CONFIG = 0x1A + __MPUREG_GYRO_CONFIG = 0x1B + __MPUREG_ACCEL_CONFIG = 0x1C + __MPUREG_ACCEL_CONFIG_2 = 0x1D + __MPUREG_LP_ACCEL_ODR = 0x1E + __MPUREG_MOT_THR = 0x1F + __MPUREG_FIFO_EN = 0x23 + __MPUREG_I2C_MST_CTRL = 0x24 + __MPUREG_I2C_SLV0_ADDR = 0x25 + __MPUREG_I2C_SLV0_REG = 0x26 + __MPUREG_I2C_SLV0_CTRL = 0x27 + __MPUREG_I2C_SLV1_ADDR = 0x28 + __MPUREG_I2C_SLV1_REG = 0x29 + __MPUREG_I2C_SLV1_CTRL = 0x2A + __MPUREG_I2C_SLV2_ADDR = 0x2B + __MPUREG_I2C_SLV2_REG = 0x2C + __MPUREG_I2C_SLV2_CTRL = 0x2D + __MPUREG_I2C_SLV3_ADDR = 0x2E + __MPUREG_I2C_SLV3_REG = 0x2F + __MPUREG_I2C_SLV3_CTRL = 0x30 + __MPUREG_I2C_SLV4_ADDR = 0x31 + __MPUREG_I2C_SLV4_REG = 0x32 + __MPUREG_I2C_SLV4_DO = 0x33 + __MPUREG_I2C_SLV4_CTRL = 0x34 + __MPUREG_I2C_SLV4_DI = 0x35 + __MPUREG_I2C_MST_STATUS = 0x36 + __MPUREG_INT_PIN_CFG = 0x37 + __MPUREG_INT_ENABLE = 0x38 + __MPUREG_ACCEL_XOUT_H = 0x3B + __MPUREG_ACCEL_XOUT_L = 0x3C + __MPUREG_ACCEL_YOUT_H = 0x3D + __MPUREG_ACCEL_YOUT_L = 0x3E + __MPUREG_ACCEL_ZOUT_H = 0x3F + __MPUREG_ACCEL_ZOUT_L = 0x40 + __MPUREG_TEMP_OUT_H = 0x41 + __MPUREG_TEMP_OUT_L = 0x42 + __MPUREG_GYRO_XOUT_H = 0x43 + __MPUREG_GYRO_XOUT_L = 0x44 + __MPUREG_GYRO_YOUT_H = 0x45 + __MPUREG_GYRO_YOUT_L = 0x46 + __MPUREG_GYRO_ZOUT_H = 0x47 + __MPUREG_GYRO_ZOUT_L = 0x48 + __MPUREG_EXT_SENS_DATA_00 = 0x49 + __MPUREG_EXT_SENS_DATA_01 = 0x4A + __MPUREG_EXT_SENS_DATA_02 = 0x4B + __MPUREG_EXT_SENS_DATA_03 = 0x4C + __MPUREG_EXT_SENS_DATA_04 = 0x4D + __MPUREG_EXT_SENS_DATA_05 = 0x4E + __MPUREG_EXT_SENS_DATA_06 = 0x4F + __MPUREG_EXT_SENS_DATA_07 = 0x50 + __MPUREG_EXT_SENS_DATA_08 = 0x51 + __MPUREG_EXT_SENS_DATA_09 = 0x52 + __MPUREG_EXT_SENS_DATA_10 = 0x53 + __MPUREG_EXT_SENS_DATA_11 = 0x54 + __MPUREG_EXT_SENS_DATA_12 = 0x55 + __MPUREG_EXT_SENS_DATA_13 = 0x56 + __MPUREG_EXT_SENS_DATA_14 = 0x57 + __MPUREG_EXT_SENS_DATA_15 = 0x58 + __MPUREG_EXT_SENS_DATA_16 = 0x59 + __MPUREG_EXT_SENS_DATA_17 = 0x5A + __MPUREG_EXT_SENS_DATA_18 = 0x5B + __MPUREG_EXT_SENS_DATA_19 = 0x5C + __MPUREG_EXT_SENS_DATA_20 = 0x5D + __MPUREG_EXT_SENS_DATA_21 = 0x5E + __MPUREG_EXT_SENS_DATA_22 = 0x5F + __MPUREG_EXT_SENS_DATA_23 = 0x60 + __MPUREG_I2C_SLV0_DO = 0x63 + __MPUREG_I2C_SLV1_DO = 0x64 + __MPUREG_I2C_SLV2_DO = 0x65 + __MPUREG_I2C_SLV3_DO = 0x66 + __MPUREG_I2C_MST_DELAY_CTRL = 0x67 + __MPUREG_SIGNAL_PATH_RESET = 0x68 + __MPUREG_MOT_DETECT_CTRL = 0x69 + __MPUREG_USER_CTRL = 0x6A + __MPUREG_PWR_MGMT_1 = 0x6B + __MPUREG_PWR_MGMT_2 = 0x6C + __MPUREG_BANK_SEL = 0x6D + __MPUREG_MEM_START_ADDR = 0x6E + __MPUREG_MEM_R_W = 0x6F + __MPUREG_DMP_CFG_1 = 0x70 + __MPUREG_DMP_CFG_2 = 0x71 + __MPUREG_FIFO_COUNTH = 0x72 + __MPUREG_FIFO_COUNTL = 0x73 + __MPUREG_FIFO_R_W = 0x74 + __MPUREG_WHOAMI = 0x75 + __MPUREG_XA_OFFSET_H = 0x77 + __MPUREG_XA_OFFSET_L = 0x78 + __MPUREG_YA_OFFSET_H = 0x7A + __MPUREG_YA_OFFSET_L = 0x7B + __MPUREG_ZA_OFFSET_H = 0x7D + __MPUREG_ZA_OFFSET_L = 0x7E + + # ---- AK8963 Reg In MPU9250 ----------------------------------------------- + + __AK8963_I2C_ADDR = 0x0C # 0x18 + __AK8963_Device_ID = 0x48 + + # Read-only Reg + __AK8963_WIA = 0x00 + __AK8963_INFO = 0x01 + __AK8963_ST1 = 0x02 + __AK8963_HXL = 0x03 + __AK8963_HXH = 0x04 + __AK8963_HYL = 0x05 + __AK8963_HYH = 0x06 + __AK8963_HZL = 0x07 + __AK8963_HZH = 0x08 + __AK8963_ST2 = 0x09 + + # Write/Read Reg + __AK8963_CNTL1 = 0x0A + __AK8963_CNTL2 = 0x0B + __AK8963_ASTC = 0x0C + __AK8963_TS1 = 0x0D + __AK8963_TS2 = 0x0E + __AK8963_I2CDIS = 0x0F + + # Read-only Reg ( ROM ) + __AK8963_ASAX = 0x10 + __AK8963_ASAY = 0x11 + __AK8963_ASAZ = 0x12 + + # Configuration bits MPU9250 + __BIT_SLEEP = 0x40 + __BIT_H_RESET = 0x80 + __BITS_CLKSEL = 0x07 + __MPU_CLK_SEL_PLLGYROX = 0x01 + __MPU_CLK_SEL_PLLGYROZ = 0x03 + __MPU_EXT_SYNC_GYROX = 0x02 + __BITS_FS_250DPS = 0x00 + __BITS_FS_500DPS = 0x08 + __BITS_FS_1000DPS = 0x10 + __BITS_FS_2000DPS = 0x18 + __BITS_FS_2G = 0x00 + __BITS_FS_4G = 0x08 + __BITS_FS_8G = 0x10 + __BITS_FS_16G = 0x18 + __BITS_FS_MASK = 0x18 + __BITS_DLPF_CFG_256HZ_NOLPF2 = 0x00 + __BITS_DLPF_CFG_188HZ = 0x01 + __BITS_DLPF_CFG_98HZ = 0x02 + __BITS_DLPF_CFG_42HZ = 0x03 + __BITS_DLPF_CFG_20HZ = 0x04 + __BITS_DLPF_CFG_10HZ = 0x05 + __BITS_DLPF_CFG_5HZ = 0x06 + __BITS_DLPF_CFG_2100HZ_NOLPF = 0x07 + __BITS_DLPF_CFG_MASK = 0x07 + __BIT_INT_ANYRD_2CLEAR = 0x10 + __BIT_RAW_RDY_EN = 0x01 + __BIT_I2C_IF_DIS = 0x10 + + __READ_FLAG = 0x80 + + # ---- Sensitivity --------------------------------------------------------- + + __MPU9250A_2g = 0.000061035156 # 0.000061035156 g/LSB + __MPU9250A_4g = 0.000122070312 # 0.000122070312 g/LSB + __MPU9250A_8g = 0.000244140625 # 0.000244140625 g/LSB + __MPU9250A_16g = 0.000488281250 # 0.000488281250 g/LSB + + __MPU9250G_250dps = 0.007633587786 # 0.007633587786 dps/LSB + __MPU9250G_500dps = 0.015267175572 # 0.015267175572 dps/LSB + __MPU9250G_1000dps = 0.030487804878 # 0.030487804878 dps/LSB + __MPU9250G_2000dps = 0.060975609756 # 0.060975609756 dps/LSB + + __MPU9250M_4800uT = 0.6 # 0.6 uT/LSB + + __MPU9250T_85degC = 0.002995177763 # 0.002995177763 degC/LSB + + __Magnetometer_Sensitivity_Scale_Factor = 0.15 + + def __init__(self, spi_bus_number=0, spi_dev_number=1): + self.bus = spidev.SpiDev() + self.spi_bus_number = spi_bus_number + self.spi_dev_number = spi_dev_number + self.gyro_divider = 0.0 + self.acc_divider = 0.0 + self.calib_data = [0.0, 0.0, 0.0] + self.magnetometer_ASA = [0.0, 0.0, 0.0] + self.temperature = 0.0 + self.gyroscope_data = [0.0, 0.0, 0.0] + self.accelerometer_data = [0.0, 0.0, 0.0] + self.magnetometer_data = [0.0, 0.0, 0.0] + + def bus_open(self): + self.bus.open(self.spi_bus_number, self.spi_dev_number) + self.bus.max_speed_hz = 10000000 + + # ----------------------------------------------------------------------------------------------- + # REGISTER READ & WRITE + # usage: use these methods to read and write MPU9250 registers over SPI + # ----------------------------------------------------------------------------------------------- + + def WriteReg(self, reg_address, data): + self.bus_open() + tx = [reg_address, data] + rx = self.bus.xfer2(tx) + self.bus.close() + return rx + + # ----------------------------------------------------------------------------------------------- + + def ReadReg(self, reg_address): + self.bus_open() + tx = [reg_address | self.__READ_FLAG, 0x00] + rx = self.bus.xfer2(tx) + self.bus.close() + return rx[1] + + # ----------------------------------------------------------------------------------------------- + + def ReadRegs(self, reg_address, length): + self.bus_open() + tx = [0] * (length + 1) + tx[0] = reg_address | self.__READ_FLAG + + rx = self.bus.xfer2(tx) + + self.bus.close() + return rx[1 : len(rx)] + + # ----------------------------------------------------------------------------------------------- + # TEST CONNECTION + # usage: call this function to know if SPI and MPU9250 are working correctly. + # returns true if mpu9250 answers + # ----------------------------------------------------------------------------------------------- + + def testConnection(self): + response = self.ReadReg(self.__MPUREG_WHOAMI) + if response == 0x71: + return True + else: + return False + + # ----------------------------------------------------------------------------------------------- + # INITIALIZATION + # usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and + # low pass filter value; suitable values are: + # BITS_DLPF_CFG_256HZ_NOLPF2 + # BITS_DLPF_CFG_188HZ + # BITS_DLPF_CFG_98HZ + # BITS_DLPF_CFG_42HZ + # BITS_DLPF_CFG_20HZ + # BITS_DLPF_CFG_10HZ + # BITS_DLPF_CFG_5HZ + # BITS_DLPF_CFG_2100HZ_NOLPF + # returns 1 if an error occurred + # ----------------------------------------------------------------------------------------------- + + def initialize(self, sample_rate_div=1, low_pass_filter=0x01): + MPU_InitRegNum = 17 + MPU_Init_Data = [[0, 0]] * MPU_InitRegNum + + MPU_Init_Data = [ + [0x80, self.__MPUREG_PWR_MGMT_1], # Reset Device + [0x01, self.__MPUREG_PWR_MGMT_1], # Clock Source + [0x00, self.__MPUREG_PWR_MGMT_2], # Enable Acc & Gyro + [ + low_pass_filter, + self.__MPUREG_CONFIG, + ], # Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz + [0x18, self.__MPUREG_GYRO_CONFIG], # +-2000dps + [0x08, self.__MPUREG_ACCEL_CONFIG], # +-4G + [ + 0x09, + self.__MPUREG_ACCEL_CONFIG_2, + ], # Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz + [0x30, self.__MPUREG_INT_PIN_CFG], + # [0x40, self.__MPUREG_I2C_MST_CTRL], # I2C Speed 348 kHz + # [0x20, self.__MPUREG_USER_CTRL], # Enable AUX + [0x20, self.__MPUREG_USER_CTRL], # I2C Master mode + [ + 0x0D, + self.__MPUREG_I2C_MST_CTRL, + ], # I2C configuration multi-master IIC 400KHz + [ + self.__AK8963_I2C_ADDR, + self.__MPUREG_I2C_SLV0_ADDR, + ], # Set the I2C slave addres of AK8963 and set for write. + # [0x09, self.__MPUREG_I2C_SLV4_CTRL], + # [0x81, self.__MPUREG_I2C_MST_DELAY_CTRL], #Enable I2C delay + [ + self.__AK8963_CNTL2, + self.__MPUREG_I2C_SLV0_REG, + ], # I2C slave 0 register address from where to begin data transfer + [0x01, self.__MPUREG_I2C_SLV0_DO], # Reset AK8963 + [0x81, self.__MPUREG_I2C_SLV0_CTRL], # Enable I2C and set 1 byte + [ + self.__AK8963_CNTL1, + self.__MPUREG_I2C_SLV0_REG, + ], # I2C slave 0 register address from where to begin data transfer + [ + 0x12, + self.__MPUREG_I2C_SLV0_DO, + ], # Register value to continuous measurement in 16bit + [0x81, self.__MPUREG_I2C_SLV0_CTRL], # Enable I2C and set 1 byte + ] + + for i in range(0, MPU_InitRegNum): + self.WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]) + time.sleep( + 0.01 + ) # I2C must slow down the write speed, otherwise it won't work + + self.set_acc_scale(self.__BITS_FS_16G) + self.set_gyro_scale(self.__BITS_FS_2000DPS) + + self.calib_mag() + + # ----------------------------------------------------------------------------------------------- + # ACCELEROMETER SCALE + # usage: call this function at startup, after initialization, to set the right range for the + # accelerometers. Suitable ranges are: + # BITS_FS_2G + # BITS_FS_4G + # BITS_FS_8G + # BITS_FS_16G + # returns the range set (2,4,8 or 16) + # ----------------------------------------------------------------------------------------------- + + def set_acc_scale(self, scale): + self.WriteReg(self.__MPUREG_ACCEL_CONFIG, scale) + if scale == self.__BITS_FS_2G: + self.acc_divider = 16384.0 + elif scale == self.__BITS_FS_4G: + self.acc_divider = 8192.0 + elif scale == self.__BITS_FS_8G: + self.acc_divider = 4096.0 + elif scale == self.__BITS_FS_16G: + self.acc_divider = 2048.0 + + temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG) + if temp_scale == self.__BITS_FS_2G: + temp_scale = 2 + elif temp_scale == self.__BITS_FS_4G: + temp_scale = 4 + elif temp_scale == self.__BITS_FS_8G: + temp_scale = 8 + elif temp_scale == self.__BITS_FS_16G: + temp_scale = 16 + + return temp_scale + + # ----------------------------------------------------------------------------------------------- + # GYROSCOPE SCALE + # usage: call this function at startup, after initialization, to set the right range for the + # gyroscopes. Suitable ranges are: + # BITS_FS_250DPS + # BITS_FS_500DPS + # BITS_FS_1000DPS + # BITS_FS_2000DPS + # returns the range set (250,500,1000 or 2000) + # ----------------------------------------------------------------------------------------------- + + def set_gyro_scale(self, scale): + self.WriteReg(self.__MPUREG_GYRO_CONFIG, scale) + if scale == self.__BITS_FS_250DPS: + self.gyro_divider = 131.0 + elif scale == self.__BITS_FS_500DPS: + self.gyro_divider = 65.6 + elif scale == self.__BITS_FS_1000DPS: + self.gyro_divider = 32.8 + elif scale == self.__BITS_FS_2000DPS: + self.gyro_divider = 16.4 + + temp_scale = self.ReadReg(self.__MPUREG_GYRO_CONFIG) + if temp_scale == self.__BITS_FS_250DPS: + temp_scale = 250 + elif temp_scale == self.__BITS_FS_500DPS: + temp_scale = 500 + elif temp_scale == self.__BITS_FS_1000DPS: + temp_scale = 1000 + elif temp_scale == self.__BITS_FS_2000DPS: + temp_scale = 2000 + + return temp_scale + + # ----------------------------------------------------------------------------------------------- + # WHO AM I? + # usage: call this function to know if SPI is working correctly. It checks the I2C address of the + # mpu9250 which should be 104 when in SPI mode. + # returns the I2C address (104) + # ----------------------------------------------------------------------------------------------- + + def whoami(self): + return self.ReadReg(self.__MPUREG_WHOAMI) + + # ----------------------------------------------------------------------------------------------- + # READ ACCELEROMETER + # usage: call this function to read accelerometer data. Axis represents selected axis: + # 0 -> X axis + # 1 -> Y axis + # 2 -> Z axis + # ----------------------------------------------------------------------------------------------- + + def read_acc(self): + response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 6) + + for i in range(0, 3): + data = self.byte_to_float(response[i * 2 : i * 2 + 2]) + self.accelerometer_data[i] = self.G_SI * data / self.acc_divider + + # ----------------------------------------------------------------------------------------------- + # READ GYROSCOPE + # usage: call this function to read gyroscope data. Axis represents selected axis: + # 0 -> X axis + # 1 -> Y axis + # 2 -> Z axis + # ----------------------------------------------------------------------------------------------- + + def read_gyro(self): + response = self.ReadRegs(self.__MPUREG_GYRO_XOUT_H, 6) + + for i in range(0, 3): + data = self.byte_to_float(response[i * 2 : i * 2 + 2]) + self.gyroscope_data[i] = (self.PI / 180) * data / self.gyro_divider + + # ----------------------------------------------------------------------------------------------- + # READ TEMPERATURE + # usage: call this function to read temperature data. + # returns the value in Celsius + # ----------------------------------------------------------------------------------------------- + + def read_temp(self): + response = self.ReadRegs(self.__MPUREG_TEMP_OUT_H, 2) + + # temp = response[0]*256.0 + response[1] + temp = self.byte_to_float(response) + self.temperature = (temp / 340.0) + 36.53 + + # ----------------------------------------------------------------------------------------------- + + def AK8963_whoami(self): + self.WriteReg( + self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG + ) # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg( + self.__MPUREG_I2C_SLV0_REG, self.__AK8963_WIA + ) # I2C slave 0 register address from where to begin data transfer + self.WriteReg( + self.__MPUREG_I2C_SLV0_CTRL, 0x81 + ) # Read 1 byte from the magnetometer + + # self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) # Enable I2C and set bytes + time.sleep(0.01) + + return self.ReadReg(self.__MPUREG_EXT_SENS_DATA_00) # Read I2C + + # ----------------------------------------------------------------------------------------------- + + def calib_mag(self): + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg( + self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG + ) + # I2C slave 0 register address from where to begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_ASAX) + # Read 3 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x83) + + time.sleep(0.01) + + response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 3) + + for i in range(0, 3): + self.magnetometer_ASA[i] = ( + (float(response[i]) - 128) / 256 + 1 + ) * self.__Magnetometer_Sensitivity_Scale_Factor + + # ----------------------------------------------------------------------------------------------- + + def read_mag(self): + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg( + self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG + ) + # I2C slave 0 register address from where to begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) + # Read 6 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) + + time.sleep(0.01) + + response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 7) + # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. + for i in range(0, 3): + data = self.byte_to_float_le(response[i * 2 : i * 2 + 2]) + self.magnetometer_data[i] = data * self.magnetometer_ASA[i] + + # ----------------------------------------------------------------------------------------------- + + def read_all(self): + # Send I2C command at first + # Set the I2C slave addres of AK8963 and set for read. + self.WriteReg( + self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG + ) + # I2C slave 0 register address from where ; //Read 7 bytes from the magnetometerto begin data transfer + self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) + # Read 7 bytes from the magnetometer + self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) + # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. + + # time.sleep(0.001) + response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 21) + + # Get Accelerometer values + for i in range(0, 3): + data = self.byte_to_float(response[i * 2 : i * 2 + 2]) + self.accelerometer_data[i] = self.G_SI * data / self.acc_divider + + # Get temperature + i = 3 + temp = self.byte_to_float(response[i * 2 : i * 2 + 2]) + self.temperature = (temp / 340.0) + 36.53 + + # Get gyroscope values + for i in range(4, 7): + data = self.byte_to_float(response[i * 2 : i * 2 + 2]) + self.gyroscope_data[i - 4] = (self.PI / 180) * data / self.gyro_divider + + # Get magnetometer values + for i in range(7, 10): + data = self.byte_to_float_le(response[i * 2 : i * 2 + 2]) + self.magnetometer_data[i - 7] = data * self.magnetometer_ASA[i - 7] + + # ----------------------------------------------------------------------------------------------- + # GET VALUES + # usage: call this functions to read and get values + # returns accel, gyro and mag values + # ----------------------------------------------------------------------------------------------- + + def getMotion9(self): + self.read_all() + m9a = self.accelerometer_data + m9g = self.gyroscope_data + m9m = self.magnetometer_data + + return m9a, m9g, m9m + + # ----------------------------------------------------------------------------------------------- + + def getMotion6(self): + self.read_acc() + self.read_gyro() + + m6a = self.accelerometer_data + m6g = self.gyroscope_data + + return m6a, m6g + + # ----------------------------------------------------------------------------------------------- + + def byte_to_float(self, input_buffer): + byte_array = array.array("B", input_buffer) + (signed_16_bit_int,) = struct.unpack(">h", byte_array) + return float(signed_16_bit_int) + + # ----------------------------------------------------------------------------------------------- + + def byte_to_float_le(self, input_buffer): + byte_array = array.array("B", input_buffer) + (signed_16_bit_int,) = struct.unpack("= 2000: + T2 = 0 + OFF2 = 0 + SENS2 = 0 + elif self.TEMP < 2000: + T2 = dT * dT / 2 ** 31 + OFF2 = 5 * ((self.TEMP - 2000) ** 2) / 2 + SENS2 = OFF2 / 2 + elif self.TEMP < -1500: + OFF2 = OFF2 + 7 * ((self.TEMP + 1500) ** 2) + SENS2 = SENS2 + 11 * (self.TEMP + 1500) ** 2 / 2 + + self.TEMP = self.TEMP - T2 + OFF = OFF - OFF2 + SENS = SENS - SENS2 + + self.PRES = (self.D1 * SENS / 2 ** 21 - OFF) / 2 ** 15 + + self.TEMP = self.TEMP / 100 # Temperature updated + self.PRES = self.PRES / 100 # Pressure updated + + def returnPressure(self): + return self.PRES + + def returnTemperature(self): + return self.TEMP + + def update(self): + self.refreshPressure() + time.sleep(0.01) # Waiting for pressure data ready + self.readPressure() + + self.refreshTemperature() + time.sleep(0.01) # Waiting for temperature data ready + self.readTemperature() + + self.calculatePressureAndTemperature() + + def test(self): + self.initialize() + self.update() + is_pressure_valid = 1000 <= self.PRES <= 1050 + is_temp_valid = -40 <= self.TEMP <= 80 + + return is_pressure_valid and is_temp_valid diff --git a/Python/navio/Common/ublox.py b/Python/navio/Common/ublox.py new file mode 100644 index 00000000..437e0d73 --- /dev/null +++ b/Python/navio/Common/ublox.py @@ -0,0 +1,1297 @@ +#!/usr/bin/env python +""" +UBlox binary protocol handling + +Copyright Andrew Tridgell, October 2012 +Released under GNU GPL version 3 or later +""" + +import struct +from datetime import datetime +import time, os +import sys + +# specify Python version +if sys.version_info[0] < 3: # we're on python 2.x.x + PYTHON_VERSION = 2 +else: + PYTHON_VERSION = 3 + +# protocol constants +PREAMBLE1 = 0xB5 +PREAMBLE2 = 0x62 + +# message classes +CLASS_NAV = 0x01 +CLASS_RXM = 0x02 +CLASS_INF = 0x04 +CLASS_ACK = 0x05 +CLASS_CFG = 0x06 +CLASS_MON = 0x0A +CLASS_AID = 0x0B +CLASS_TIM = 0x0D +CLASS_ESF = 0x10 + +# ACK messages +MSG_ACK_NACK = 0x00 +MSG_ACK_ACK = 0x01 + +# NAV messages +MSG_NAV_POSECEF = 0x1 +MSG_NAV_POSLLH = 0x2 +MSG_NAV_STATUS = 0x3 +MSG_NAV_DOP = 0x4 +MSG_NAV_SOL = 0x6 +MSG_NAV_POSUTM = 0x8 +MSG_NAV_VELNED = 0x12 +MSG_NAV_VELECEF = 0x11 +MSG_NAV_TIMEGPS = 0x20 +MSG_NAV_TIMEUTC = 0x21 +MSG_NAV_CLOCK = 0x22 +MSG_NAV_SVINFO = 0x30 +MSG_NAV_AOPSTATUS = 0x60 +MSG_NAV_DGPS = 0x31 +MSG_NAV_DOP = 0x04 +MSG_NAV_EKFSTATUS = 0x40 +MSG_NAV_SBAS = 0x32 +MSG_NAV_SOL = 0x06 +MSG_NAV_PVT = 0x07 + +# RXM messages +MSG_RXM_RAW = 0x10 +MSG_RXM_SFRB = 0x11 +MSG_RXM_SVSI = 0x20 +MSG_RXM_EPH = 0x31 +MSG_RXM_ALM = 0x30 +MSG_RXM_PMREQ = 0x41 + +# AID messages +MSG_AID_ALM = 0x30 +MSG_AID_EPH = 0x31 +MSG_AID_ALPSRV = 0x32 +MSG_AID_AOP = 0x33 +MSG_AID_DATA = 0x10 +MSG_AID_ALP = 0x50 +MSG_AID_DATA = 0x10 +MSG_AID_HUI = 0x02 +MSG_AID_INI = 0x01 +MSG_AID_REQ = 0x00 + +# CFG messages +MSG_CFG_PRT = 0x00 +MSG_CFG_ANT = 0x13 +MSG_CFG_DAT = 0x06 +MSG_CFG_EKF = 0x12 +MSG_CFG_ESFGWT = 0x29 +MSG_CFG_CFG = 0x09 +MSG_CFG_USB = 0x1B +MSG_CFG_RATE = 0x08 +MSG_CFG_SET_RATE = 0x01 +MSG_CFG_NAV5 = 0x24 +MSG_CFG_FXN = 0x0E +MSG_CFG_INF = 0x02 +MSG_CFG_ITFM = 0x39 +MSG_CFG_MSG = 0x01 +MSG_CFG_NAVX5 = 0x23 +MSG_CFG_NMEA = 0x17 +MSG_CFG_NVS = 0x22 +MSG_CFG_PM2 = 0x3B +MSG_CFG_PM = 0x32 +MSG_CFG_RINV = 0x34 +MSG_CFG_RST = 0x04 +MSG_CFG_RXM = 0x11 +MSG_CFG_SBAS = 0x16 +MSG_CFG_TMODE2 = 0x3D +MSG_CFG_TMODE = 0x1D +MSG_CFG_TPS = 0x31 +MSG_CFG_TP = 0x07 +MSG_CFG_GNSS = 0x3E + +# ESF messages +MSG_ESF_MEAS = 0x02 +MSG_ESF_STATUS = 0x10 + +# INF messages +MSG_INF_DEBUG = 0x04 +MSG_INF_ERROR = 0x00 +MSG_INF_NOTICE = 0x02 +MSG_INF_TEST = 0x03 +MSG_INF_WARNING = 0x01 + +# MON messages +MSG_MON_SCHD = 0x01 +MSG_MON_HW = 0x09 +MSG_MON_HW2 = 0x0B +MSG_MON_IO = 0x02 +MSG_MON_MSGPP = 0x06 +MSG_MON_RXBUF = 0x07 +MSG_MON_RXR = 0x21 +MSG_MON_TXBUF = 0x08 +MSG_MON_VER = 0x04 + +# TIM messages +MSG_TIM_TP = 0x01 +MSG_TIM_TM2 = 0x03 +MSG_TIM_SVIN = 0x04 +MSG_TIM_VRFY = 0x06 + +# port IDs +PORT_DDC = 0 +PORT_SERIAL1 = 1 +PORT_SERIAL2 = 2 +PORT_USB = 3 +PORT_SPI = 4 + +# dynamic models +DYNAMIC_MODEL_PORTABLE = 0 +DYNAMIC_MODEL_STATIONARY = 2 +DYNAMIC_MODEL_PEDESTRIAN = 3 +DYNAMIC_MODEL_AUTOMOTIVE = 4 +DYNAMIC_MODEL_SEA = 5 +DYNAMIC_MODEL_AIRBORNE1G = 6 +DYNAMIC_MODEL_AIRBORNE2G = 7 +DYNAMIC_MODEL_AIRBORNE4G = 8 + +# reset items +RESET_HOT = 0 +RESET_WARM = 1 +RESET_COLD = 0xFFFF + +RESET_HW = 0 +RESET_SW = 1 +RESET_SW_GPS = 2 +RESET_HW_GRACEFUL = 4 +RESET_GPS_STOP = 8 +RESET_GPS_START = 9 + + +class UBloxError(Exception): + """Ublox error class""" + + def __init__(self, msg): + Exception.__init__(self, msg) + self.message = msg + + +class UBloxAttrDict(dict): + """allow dictionary members as attributes""" + + def __init__(self): + dict.__init__(self) + + def __getattr__(self, name): + try: + return self.__getitem__(name) + except KeyError: + raise AttributeError(name) + + def __setattr__(self, name, value): + if self.__dict__.has_key(name): + # allow set on normal attributes + dict.__setattr__(self, name, value) + else: + self.__setitem__(name, value) + + +def ArrayParse(field): + """parse an array descriptor""" + arridx = field.find("[") + if arridx == -1: + return (field, -1) + alen = int(field[arridx + 1 : -1]) + fieldname = field[:arridx] + return (fieldname, alen) + + +class UBloxDescriptor: + """class used to describe the layout of a UBlox message""" + + def __init__( + self, name, msg_format, fields=[], count_field=None, format2=None, fields2=None + ): + self.name = name + self.msg_format = msg_format + self.fields = fields + self.count_field = count_field + self.format2 = format2 + self.fields2 = fields2 + + def getf(self, fmt, buf, size): + f = list(struct.unpack(fmt, buf[:size])) + return f + + def unpack(self, msg): + """unpack a UBloxMessage, creating the .fields and ._recs attributes in msg""" + msg._fields = {} + + # unpack main message blocks. A comm + formats = self.msg_format.split(",") + buf = msg._buf[6:-2] + count = 0 + msg._recs = [] + fields = self.fields[:] + + for fmt in formats: + size1 = struct.calcsize(fmt) + if size1 > len(buf): + raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf))) + f1 = self.getf(fmt, buf, size1) + + i = 0 + while i < len(f1): + field = fields.pop(0) + (fieldname, alen) = ArrayParse(field) + if alen == -1: + msg._fields[fieldname] = f1[i] + if self.count_field == fieldname: + count = int(f1[i]) + i += 1 + else: + msg._fields[fieldname] = [0] * alen + for a in range(alen): + msg._fields[fieldname][a] = f1[i] + i += 1 + buf = buf[size1:] + if len(buf) == 0: + break + + if self.count_field == "_remaining": + count = len(buf) / struct.calcsize(self.format2) + + if count == 0: + msg._unpacked = True + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + return + + size2 = struct.calcsize(self.format2) + for c in range(count): + r = UBloxAttrDict() + if size2 > len(buf): + raise UBloxError("INVALID_SIZE=%u, " % len(buf)) + f2 = self.getf(self.format2, buf, size2) + + for i in range(len(self.fields2)): + r[self.fields2[i]] = f2[i] + buf = buf[size2:] + msg._recs.append(r) + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + msg._unpacked = True + + def pack(self, msg, msg_class=None, msg_id=None): + """pack a UBloxMessage from the .fields and ._recs attributes in msg""" + f1 = [] + if msg_class is None: + msg_class = msg.msg_class() + if msg_id is None: + msg_id = msg.msg_id() + msg._buf = "" + + fields = self.fields[:] + for f in fields: + (fieldname, alen) = ArrayParse(f) + if not fieldname in msg._fields: + break + if alen == -1: + f1.append(msg._fields[fieldname]) + else: + for a in range(alen): + f1.append(msg._fields[fieldname][a]) + try: + # try full length message + fmt = self.msg_format.replace(",", "") + msg._buf = struct.pack(fmt, *tuple(f1)) + except Exception as e: + # try without optional part + fmt = self.msg_format.split(",")[0] + msg._buf = struct.pack(fmt, *tuple(f1)) + + length = len(msg._buf) + if msg._recs: + length += len(msg._recs) * struct.calcsize(self.format2) + header = struct.pack("= level: + print(msg) + + def unpack(self): + """unpack a message""" + if not self.valid(): + raise UBloxError("INVALID MESSAGE") + type = self.msg_type() + if not type in msg_types: + raise UBloxError( + "Unknown message %s length=%u" % (str(type), len(self._buf)) + ) + msg_types[type].unpack(self) + + def pack(self): + """pack a message""" + if not self.valid(): + raise UbloxError("INVALID MESSAGE") + type = self.msg_type() + if not type in msg_types: + raise UBloxError("Unknown message %s" % str(type)) + msg_types[type].pack(self) + + def name(self): + """return the short string name for a message""" + if not self.valid(): + raise UbloxError("INVALID MESSAGE") + type = self.msg_type() + if not type in msg_types: + raise UBloxError( + "Unknown message %s length=%u" % (str(type), len(self._buf)) + ) + return msg_types[type].name + + if PYTHON_VERSION == 2: + + def msg_class(self): + """return the message class""" + return ord(self._buf[2]) + + def msg_id(self): + """return the message id within the class""" + return ord(self._buf[3]) + + else: + + def msg_class(self): + """return the message class""" + return self._buf[2] + + def msg_id(self): + """return the message id within the class""" + return self._buf[3] + + def msg_type(self): + """return the message type tuple (class, id)""" + return (self.msg_class(), self.msg_id()) + + def msg_length(self): + """return the payload length""" + (payload_length,) = struct.unpack(" 0 and ord(self._buf[0]) != PREAMBLE1: + return False + if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2: + self.debug(1, "bad pre2") + return False + else: + if len(self._buf) > 0 and (self._buf[0]) != PREAMBLE1: + return False + if len(self._buf) > 1 and (self._buf[1]) != PREAMBLE2: + self.debug(1, "bad pre2") + return False + + if self.needed_bytes() == 0 and not self.valid(): + if len(self._buf) > 8: + self.debug( + 1, + "bad checksum len=%u needed=%u" + % (len(self._buf), self.needed_bytes()), + ) + else: + self.debug( + 1, + "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()), + ) + return False + return True + + def add(self, bytes): + """add some bytes to a message""" + + self._buf += bytes + while not self.valid_so_far() and len(self._buf) > 0: + """handle corrupted streams""" + self._buf = self._buf[1:] + if self.needed_bytes() < 0: + self._buf = b"" + + def checksum(self, data=None): + """return a checksum tuple for a message""" + if data is None: + data = self._buf[2:-2] + cs = 0 + ck_a = 0 + ck_b = 0 + for i in data: + if type(i) is str: + + ck_a = (ck_a + ord(i)) & 0xFF + else: + ck_a = (ck_a + i) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return (ck_a, ck_b) + + def valid_checksum(self): + """check if the checksum is OK""" + (ck_a, ck_b) = self.checksum() + d = self._buf[2:-2] + (ck_a2, ck_b2) = struct.unpack("= 8 and self.needed_bytes() == 0 and self.valid_checksum() + ) + + +class UBlox: + """main UBlox control class. + + port can be a file (for reading only) or a serial device + """ + + def __init__(self, port, baudrate=115200, timeout=0): + + self.serial_device = port + self.baudrate = baudrate + self.use_sendrecv = False + self.read_only = False + self.use_xfer = False + self.debug_level = 0 + + if self.serial_device.startswith("tcp:"): + import socket + + a = self.serial_device.split(":") + destination_addr = (a[1], int(a[2])) + self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.dev.connect(destination_addr) + self.dev.setblocking(1) + self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) + self.use_sendrecv = True + elif os.path.isfile(self.serial_device): + self.read_only = True + self.dev = open(self.serial_device, mode="rb") + if self.serial_device.startswith("spi:"): + import spidev + + bus, cs = map(int, self.serial_device.split(":")[1].split(".")) + # print(bus, cs) + self.use_xfer = True + self.dev = spidev.SpiDev() + self.dev.open(bus, cs) + # We reuse baudrate parameter but it's difficult to get default paramaters right. So it's better to specify them explicitly + self.dev.max_speed_hz = baudrate + else: + import serial + + self.dev = serial.Serial( + self.serial_device, + baudrate=self.baudrate, + dsrdtr=False, + rtscts=False, + xonxoff=False, + timeout=timeout, + ) + self.logfile = None + self.log = None + self.preferred_dynamic_model = None + self.preferred_usePPP = None + self.preferred_dgps_timeout = None + + def close(self): + """close the device""" + self.dev.close() + self.dev = None + + def set_debug(self, debug_level): + """set debug level""" + self.debug_level = debug_level + + def debug(self, level, msg): + """write a debug message""" + if self.debug_level >= level: + print(msg) + + def set_logfile(self, logfile, append=False): + """setup logging to a file""" + if self.log is not None: + self.log.close() + self.log = None + self.logfile = logfile + if self.logfile is not None: + if append: + mode = "ab" + else: + mode = "wb" + self.log = open(self.logfile, mode=mode) + + def set_preferred_dynamic_model(self, model): + """set the preferred dynamic model for receiver""" + self.preferred_dynamic_model = model + if model is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_dgps_timeout(self, timeout): + """set the preferred DGPS timeout for receiver""" + self.preferred_dgps_timeout = timeout + if timeout is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_usePPP(self, usePPP): + """set the preferred usePPP setting for the receiver""" + if usePPP is None: + self.preferred_usePPP = None + return + self.preferred_usePPP = int(usePPP) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def nmea_checksum(self, msg): + d = msg[1:] + cs = 0 + for i in d: + cs ^= ord(i) + return cs + + def write(self, buf): + """write some bytes""" + if not self.read_only: + if self.use_sendrecv: + return self.dev.send(buf) + elif self.use_xfer: + spiBuf = [] # form buf + for b in buf: + if type(b) is str: + + spiBuf.append(ord(b)) + else: + spiBuf.append(b) + return self.dev.xfer2(spiBuf) + return self.dev.write(buf) + + def read(self, n): + """read some bytes""" + + if self.use_sendrecv: + import socket + + try: + buf = self.dev.recv(n) + return buf + except socket.error as e: + return b"" + if self.use_xfer: + buf = self.dev.readbytes(n) + return buf + + buf = self.dev.read(n) + return buf + + def send_nmea(self, msg): + if not self.read_only: + s = msg + "*%02X" % self.nmea_checksum(msg) + + if PYTHON_VERSION == 2: + b = bytearray() + b.extend(s) + else: + b = bytearray() + b.extend(map(ord, s)) + self.write(b) + + def set_binary(self): + """put a UBlox into binary mode using a NMEA string""" + if not self.read_only: + print("try set binary at %u" % self.baudrate) + self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate) + + def seek_percent(self, pct): + """seek to the given percentage of a file""" + self.dev.seek(0, 2) + filesize = self.dev.tell() + self.dev.seek(pct * 0.01 * filesize) + + def special_handling(self, msg): + """handle automatic configuration changes""" + if msg.name() == "CFG_NAV5": + msg.unpack() + sendit = False + pollit = False + if ( + self.preferred_dynamic_model is not None + and msg.dynModel != self.preferred_dynamic_model + ): + msg.dynModel = self.preferred_dynamic_model + sendit = True + pollit = True + if ( + self.preferred_dgps_timeout is not None + and msg.dgpsTimeOut != self.preferred_dgps_timeout + ): + msg.dgpsTimeOut = self.preferred_dgps_timeout + self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut) + sendit = True + # we don't re-poll for this one, as some receivers refuse to set it + if sendit: + msg.pack() + self.send(msg) + if pollit: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + if msg.name() == "CFG_NAVX5" and self.preferred_usePPP is not None: + msg.unpack() + if msg.usePPP != self.preferred_usePPP: + msg.usePPP = self.preferred_usePPP + msg.mask = 1 << 13 + msg.pack() + self.send(msg) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def receive_message_nonblocking(self, seconds=5): + """nonblocking receive of one ublox message""" + with Timeout(seconds=seconds): + return self.receive_message() + + def receive_message(self, ignore_eof=False): + """blocking receive of one ublox message""" + msg = UBloxMessage() + while True: + n = msg.needed_bytes() + b = self.read(n) + if not b: + if ignore_eof: + time.sleep(0.01) + continue + return None + if self.use_xfer: + if PYTHON_VERSION == 3: + bb = bytearray() + for c in b: + bb.append(c) + b = bb + else: + b = "".join([chr(c) for c in b]) # here str + msg.add(b) + if self.log is not None: + self.log.write(b) + self.log.flush() + if msg.valid(): + self.special_handling(msg) + return msg + + def receive_message_noerror(self, ignore_eof=False): + """blocking receive of one ublox message, ignoring errors""" + try: + return self.receive_message(ignore_eof=ignore_eof) + except UBloxError as e: + print(e) + return None + except OSError as e: + # Occasionally we get hit with 'resource temporarily unavailable' + # messages here on the serial device, catch them too. + print(e) + return None + + def send(self, msg): + """send a preformatted ublox message""" + if not msg.valid(): + self.debug(1, "invalid send") + return + if not self.read_only: + self.write(msg._buf) + + def send_message(self, msg_class, msg_id, payload): + """send a ublox message with class, id and payload""" + msg = UBloxMessage() + msg._buf = struct.pack(" /dev/null"], shell = True) + ret = sub.call(["ps -AT | grep -c ap-timer > /dev/null"], shell=True) if ret <= 0: sys.exit("APM is running. Can't launch the example") + + +def get_navio_version(): + file = open("/sys/firmware/devicetree/base/hat/product_id") + version = "NAVIO" if file.readline(6) == "0x0001" else "NAVIO2" + return version diff --git a/Python/navio/Navio/ADC.py b/Python/navio/Navio/ADC.py new file mode 100644 index 00000000..30a87421 --- /dev/null +++ b/Python/navio/Navio/ADC.py @@ -0,0 +1,35 @@ +from adafruit_ads1x15 import ADS1x15 + + +class ADC: + ADS1015 = 0x00 # 12-bit ADC + ADS1115 = 0x01 # 16-bit ADC + + # Select the gain + # gain = 6144 # +/- 6.144V + gain = 4096 # +/- 4.096V + # gain = 2048 # +/- 2.048V + # gain = 1024 # +/- 1.024V + # gain = 512 # +/- 0.512V + # gain = 256 # +/- 0.256V + + # Select the sample rate + # sps = 8 # 8 samples per second + # sps = 16 # 16 samples per second + # sps = 32 # 32 samples per second + # sps = 64 # 64 samples per second + # sps = 128 # 128 samples per second + sps = 250 # 250 samples per second + # sps = 475 # 475 samples per second + # sps = 860 # 860 samples per second + + # Initialise the ADC using the default mode (use default I2C address) + # Set this to ADS1015 or ADS1115 depending on the ADC you are using! + + channel_count = 4 + + def __init__(self): + self.adc = ADS1x15(ic=self.ADS1115) + + def read(self, ch): + return self.adc.readADCSingleEnded(ch, self.gain, self.sps) diff --git a/Python/navio/Navio/Led.py b/Python/navio/Navio/Led.py new file mode 100644 index 00000000..fcdf982f --- /dev/null +++ b/Python/navio/Navio/Led.py @@ -0,0 +1,52 @@ +from .adafruit_pwm_servo_driver import PWM +import RPi.GPIO as GPIO + + +class Led: + def __init__(self): + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(27, GPIO.OUT) + GPIO.output(27, GPIO.LOW) + + self.pwm = PWM(0x40, debug=False) + + # Set frequency to 60 Hz + self.pwm.setPWMFreq(60) + + # set initial color + self.pwm.setPWM(0, 0, 0) + self.pwm.setPWM(1, 0, 0) + self.pwm.setPWM(2, 0, 0) + + def setColor(self, color): + if color == "Yellow": + self.pwm.setPWM(0, 0, 4095) + self.pwm.setPWM(1, 0, 0) + self.pwm.setPWM(2, 0, 0) + + elif color == "Green": + self.pwm.setPWM(0, 0, 4095) + self.pwm.setPWM(1, 0, 0) + self.pwm.setPWM(2, 0, 4095) + + elif color == "Cyan": + self.pwm.setPWM(0, 0, 0) + self.pwm.setPWM(1, 0, 0) + self.pwm.setPWM(2, 0, 4095) + + elif color == "Magenta": + self.pwm.setPWM(0, 0, 0) + self.pwm.setPWM(1, 0, 4095) + self.pwm.setPWM(2, 0, 0) + + elif color == "Red": + self.pwm.setPWM(0, 0, 4095) + self.pwm.setPWM(1, 0, 4095) + self.pwm.setPWM(2, 0, 0) + + elif color == "Blue": + self.pwm.setPWM(0, 0, 0) + self.pwm.setPWM(1, 0, 4095) + self.pwm.setPWM(2, 0, 4095) diff --git a/Python/navio/Navio/RCInput.py b/Python/navio/Navio/RCInput.py new file mode 100644 index 00000000..f8da7986 --- /dev/null +++ b/Python/navio/Navio/RCInput.py @@ -0,0 +1,43 @@ +import RPi.GPIO as GPIO +import pigpio + + +class RCInput: + + PPM_SYNC_LENGTH = 4000 + + CHANNEL_COUNT = 8 + channels = [0, 0, 0, 0, 0, 0, 0, 0] + + def cbf(self, gpio, level, tick): + if level == 0: + deltaTime = tick - self.previousTick + self.previousTick = tick + + if deltaTime >= self.PPM_SYNC_LENGTH: + self.currentchannel = 0 + else: + if self.currentchannel < self.CHANNEL_COUNT: + self.channels[self.currentchannel] = deltaTime + self.currentchannel += 1 + + def __init__(self): + + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(27, GPIO.OUT) + GPIO.output(27, GPIO.LOW) + + self.pi = pigpio.pi() + self.pi.set_mode(4, pigpio.INPUT) + + self.currentchannel = 0 + self.previousTick = self.pi.get_current_tick() + self.cb1 = self.pi.callback(4, pigpio.EITHER_EDGE, self.cbf) + + def read(self, ch): + try: + return self.channels[ch] + except IndexError: + print("Channel number too large") + exit(1) diff --git a/Python/navio/Navio/RCOutput.py b/Python/navio/Navio/RCOutput.py new file mode 100644 index 00000000..d885e352 --- /dev/null +++ b/Python/navio/Navio/RCOutput.py @@ -0,0 +1,44 @@ +from navio.Navio.adafruit_pwm_servo_driver import PWM +import RPi.GPIO as GPIO + +import math + + +class RCOutput: + + PCA9685_DEFAULT_ADDRESS = 0x40 + + def __init__(self, pwm_output): + self.NAVIO_RCOUTPUT = pwm_output + 3 + self.is_initialized = False + + def __enter__(self): + self.initialize() + return self + + def __exit__(self): + GPIO.output(27, GPIO.HIGH) + self.is_initialized = False + + def initialize(self): + GPIO.setmode(GPIO.BCM) + GPIO.setwarnings(False) + GPIO.setup(27, GPIO.OUT) + GPIO.output(27, GPIO.LOW) + self.is_initialized = True + + def enable(self): + self.pwm = PWM(0x40, debug=False) + self.pwm.setPWMFreq(self.frequency) + + def set_period(self, period): + + if not self.is_initialized: + raise RuntimeError("PWM not initialized. Call initialize first") + self.frequency = period + + def set_duty_cycle(self, ch): + ch = math.trunc((ch * 4096.0) / (1000.0 / self.frequency) - 1) + if not self.is_initialized: + raise RuntimeError("PWM not initialized. Call initialize first") + self.pwm.setPWM(self.NAVIO_RCOUTPUT, 0, ch) diff --git a/Python/navio/Navio/__init__.py b/Python/navio/Navio/__init__.py new file mode 100644 index 00000000..a9a3924e --- /dev/null +++ b/Python/navio/Navio/__init__.py @@ -0,0 +1,5 @@ +from .adafruit_ads1x15 import ADS1x15 +from .Led import Led +from .ADC import ADC +from .RCInput import RCInput +from .RCOutput import RCOutput diff --git a/Python/navio/Navio/adafruit_ads1x15.py b/Python/navio/Navio/adafruit_ads1x15.py new file mode 100644 index 00000000..3c1bcac1 --- /dev/null +++ b/Python/navio/Navio/adafruit_ads1x15.py @@ -0,0 +1,773 @@ +#!/usr/bin/python + +import time +import smbus +from adafruit_i2c import Adafruit_I2C + +# =========================================================================== +# ADS1x15 Class +# +# Originally written by K. Townsend, Adafruit (https://github.com/adafruit/Adafruit-Raspberry-Pi-Python-Code/tree/master/Adafruit_ADS1x15) +# Updates and new functions implementation by Pedro Villanueva, 03/2013. +# The only error in the original code was in line 57: +# __ADS1015_REG_CONFIG_DR_920SPS = 0x0050 +# should be +# __ADS1015_REG_CONFIG_DR_920SPS = 0x0060 +# +# NOT IMPLEMENTED: Conversion ready pin, page 15 datasheet. +# =========================================================================== + + +class ADS1x15: + i2c = None + + # IC Identifiers + __IC_ADS1015 = 0x00 + __IC_ADS1115 = 0x01 + + # Pointer Register + __ADS1015_REG_POINTER_MASK = 0x03 + __ADS1015_REG_POINTER_CONVERT = 0x00 + __ADS1015_REG_POINTER_CONFIG = 0x01 + __ADS1015_REG_POINTER_LOWTHRESH = 0x02 + __ADS1015_REG_POINTER_HITHRESH = 0x03 + + # Config Register + __ADS1015_REG_CONFIG_OS_MASK = 0x8000 + __ADS1015_REG_CONFIG_OS_SINGLE = 0x8000 # Write: Set to start a single-conversion + __ADS1015_REG_CONFIG_OS_BUSY = ( + 0x0000 # Read: Bit = 0 when conversion is in progress + ) + __ADS1015_REG_CONFIG_OS_NOTBUSY = ( + 0x8000 # Read: Bit = 1 when device is not performing a conversion + ) + + __ADS1015_REG_CONFIG_MUX_MASK = 0x7000 + __ADS1015_REG_CONFIG_MUX_DIFF_0_1 = ( + 0x0000 # Differential P = AIN0, N = AIN1 (default) + ) + __ADS1015_REG_CONFIG_MUX_DIFF_0_3 = 0x1000 # Differential P = AIN0, N = AIN3 + __ADS1015_REG_CONFIG_MUX_DIFF_1_3 = 0x2000 # Differential P = AIN1, N = AIN3 + __ADS1015_REG_CONFIG_MUX_DIFF_2_3 = 0x3000 # Differential P = AIN2, N = AIN3 + __ADS1015_REG_CONFIG_MUX_SINGLE_0 = 0x4000 # Single-ended AIN0 + __ADS1015_REG_CONFIG_MUX_SINGLE_1 = 0x5000 # Single-ended AIN1 + __ADS1015_REG_CONFIG_MUX_SINGLE_2 = 0x6000 # Single-ended AIN2 + __ADS1015_REG_CONFIG_MUX_SINGLE_3 = 0x7000 # Single-ended AIN3 + + __ADS1015_REG_CONFIG_PGA_MASK = 0x0E00 + __ADS1015_REG_CONFIG_PGA_6_144V = 0x0000 # +/-6.144V range + __ADS1015_REG_CONFIG_PGA_4_096V = 0x0200 # +/-4.096V range + __ADS1015_REG_CONFIG_PGA_2_048V = 0x0400 # +/-2.048V range (default) + __ADS1015_REG_CONFIG_PGA_1_024V = 0x0600 # +/-1.024V range + __ADS1015_REG_CONFIG_PGA_0_512V = 0x0800 # +/-0.512V range + __ADS1015_REG_CONFIG_PGA_0_256V = 0x0A00 # +/-0.256V range + + __ADS1015_REG_CONFIG_MODE_MASK = 0x0100 + __ADS1015_REG_CONFIG_MODE_CONTIN = 0x0000 # Continuous conversion mode + __ADS1015_REG_CONFIG_MODE_SINGLE = 0x0100 # Power-down single-shot mode (default) + + __ADS1015_REG_CONFIG_DR_MASK = 0x00E0 + __ADS1015_REG_CONFIG_DR_128SPS = 0x0000 # 128 samples per second + __ADS1015_REG_CONFIG_DR_250SPS = 0x0020 # 250 samples per second + __ADS1015_REG_CONFIG_DR_490SPS = 0x0040 # 490 samples per second + __ADS1015_REG_CONFIG_DR_920SPS = 0x0060 # 920 samples per second + __ADS1015_REG_CONFIG_DR_1600SPS = 0x0080 # 1600 samples per second (default) + __ADS1015_REG_CONFIG_DR_2400SPS = 0x00A0 # 2400 samples per second + __ADS1015_REG_CONFIG_DR_3300SPS = 0x00C0 # 3300 samples per second (also 0x00E0) + + __ADS1115_REG_CONFIG_DR_8SPS = 0x0000 # 8 samples per second + __ADS1115_REG_CONFIG_DR_16SPS = 0x0020 # 16 samples per second + __ADS1115_REG_CONFIG_DR_32SPS = 0x0040 # 32 samples per second + __ADS1115_REG_CONFIG_DR_64SPS = 0x0060 # 64 samples per second + __ADS1115_REG_CONFIG_DR_128SPS = 0x0080 # 128 samples per second + __ADS1115_REG_CONFIG_DR_250SPS = 0x00A0 # 250 samples per second (default) + __ADS1115_REG_CONFIG_DR_475SPS = 0x00C0 # 475 samples per second + __ADS1115_REG_CONFIG_DR_860SPS = 0x00E0 # 860 samples per second + + __ADS1015_REG_CONFIG_CMODE_MASK = 0x0010 + __ADS1015_REG_CONFIG_CMODE_TRAD = ( + 0x0000 # Traditional comparator with hysteresis (default) + ) + __ADS1015_REG_CONFIG_CMODE_WINDOW = 0x0010 # Window comparator + + __ADS1015_REG_CONFIG_CPOL_MASK = 0x0008 + __ADS1015_REG_CONFIG_CPOL_ACTVLOW = ( + 0x0000 # ALERT/RDY pin is low when active (default) + ) + __ADS1015_REG_CONFIG_CPOL_ACTVHI = 0x0008 # ALERT/RDY pin is high when active + + __ADS1015_REG_CONFIG_CLAT_MASK = ( + 0x0004 # Determines if ALERT/RDY pin latches once asserted + ) + __ADS1015_REG_CONFIG_CLAT_NONLAT = 0x0000 # Non-latching comparator (default) + __ADS1015_REG_CONFIG_CLAT_LATCH = 0x0004 # Latching comparator + + __ADS1015_REG_CONFIG_CQUE_MASK = 0x0003 + __ADS1015_REG_CONFIG_CQUE_1CONV = 0x0000 # Assert ALERT/RDY after one conversions + __ADS1015_REG_CONFIG_CQUE_2CONV = 0x0001 # Assert ALERT/RDY after two conversions + __ADS1015_REG_CONFIG_CQUE_4CONV = 0x0002 # Assert ALERT/RDY after four conversions + __ADS1015_REG_CONFIG_CQUE_NONE = ( + 0x0003 # Disable the comparator and put ALERT/RDY in high state (default) + ) + + # Dictionaries with the sampling speed values + # These simplify and clean the code (avoid the abuse of if/elif/else clauses) + spsADS1115 = { + 8: __ADS1115_REG_CONFIG_DR_8SPS, + 16: __ADS1115_REG_CONFIG_DR_16SPS, + 32: __ADS1115_REG_CONFIG_DR_32SPS, + 64: __ADS1115_REG_CONFIG_DR_64SPS, + 128: __ADS1115_REG_CONFIG_DR_128SPS, + 250: __ADS1115_REG_CONFIG_DR_250SPS, + 475: __ADS1115_REG_CONFIG_DR_475SPS, + 860: __ADS1115_REG_CONFIG_DR_860SPS, + } + spsADS1015 = { + 128: __ADS1015_REG_CONFIG_DR_128SPS, + 250: __ADS1015_REG_CONFIG_DR_250SPS, + 490: __ADS1015_REG_CONFIG_DR_490SPS, + 920: __ADS1015_REG_CONFIG_DR_920SPS, + 1600: __ADS1015_REG_CONFIG_DR_1600SPS, + 2400: __ADS1015_REG_CONFIG_DR_2400SPS, + 3300: __ADS1015_REG_CONFIG_DR_3300SPS, + } + # Dictionariy with the programable gains + pgaADS1x15 = { + 6144: __ADS1015_REG_CONFIG_PGA_6_144V, + 4096: __ADS1015_REG_CONFIG_PGA_4_096V, + 2048: __ADS1015_REG_CONFIG_PGA_2_048V, + 1024: __ADS1015_REG_CONFIG_PGA_1_024V, + 512: __ADS1015_REG_CONFIG_PGA_0_512V, + 256: __ADS1015_REG_CONFIG_PGA_0_256V, + } + + # Constructor + def __init__(self, address=0x48, ic=__IC_ADS1015, debug=False): + # Depending on if you have an old or a new Raspberry Pi, you + # may need to change the I2C bus. Older Pis use SMBus 0, + # whereas new Pis use SMBus 1. If you see an error like: + # 'Error accessing 0x48: Check your I2C address ' + # change the SMBus number in the initializer below! + self.i2c = Adafruit_I2C(address) + self.address = address + self.debug = debug + + # Make sure the IC specified is valid + if (ic < self.__IC_ADS1015) | (ic > self.__IC_ADS1115): + if self.debug: + print "ADS1x15: Invalid IC specfied: %h" % ic + return -1 + else: + self.ic = ic + + # Set pga value, so that getLastConversionResult() can use it, + # any function that accepts a pga value must update this. + self.pga = 6144 + + def readADCSingleEnded(self, channel=0, pga=6144, sps=250): + "Gets a single-ended ADC reading from the specified channel in mV. \ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see datasheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + + # With invalid channel return -1 + if channel > 3: + if self.debug: + print "ADS1x15: Invalid channel specified: %d" % channel + return -1 + + # Disable comparator, Non-latching, Alert/Rdy active low + # traditional comparator, single-shot mode + config = ( + self.__ADS1015_REG_CONFIG_CQUE_NONE + | self.__ADS1015_REG_CONFIG_CLAT_NONLAT + | self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + | self.__ADS1015_REG_CONFIG_CMODE_TRAD + | self.__ADS1015_REG_CONFIG_MODE_SINGLE + ) + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set the channel to be converted + if channel == 3: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3 + elif channel == 2: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2 + elif channel == 1: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1 + else: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0 + + # Set 'start single-conversion' bit + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write config register to the ADC + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + + # Wait for the ADC conversion to complete + # The minimum delay depends on the sps: delay >= 1/sps + # We add 0.1ms to be sure + delay = 1.0 / sps + 0.0001 + time.sleep(delay) + + # Read the conversion results + result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2) + if self.ic == self.__IC_ADS1015: + # Shift right 4 bits for the 12-bit ADS1015 and convert to mV + return (((result[0] << 8) | (result[1] & 0xFF)) >> 4) * pga / 2048.0 + else: + # Return a mV value for the ADS1115 + # (Take signed values into account as well) + val = (result[0] << 8) | (result[1]) + if val > 0x7FFF: + return (val - 0xFFFF) * pga / 32768.0 + else: + return ((result[0] << 8) | (result[1])) * pga / 32768.0 + + def readADCDifferential(self, chP=0, chN=1, pga=6144, sps=250): + "Gets a differential ADC reading from channels chP and chN in mV. \ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see data sheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + + # Disable comparator, Non-latching, Alert/Rdy active low + # traditional comparator, single-shot mode + config = ( + self.__ADS1015_REG_CONFIG_CQUE_NONE + | self.__ADS1015_REG_CONFIG_CLAT_NONLAT + | self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + | self.__ADS1015_REG_CONFIG_CMODE_TRAD + | self.__ADS1015_REG_CONFIG_MODE_SINGLE + ) + + # Set channels + if (chP == 0) & (chN == 1): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1 + elif (chP == 0) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3 + elif (chP == 2) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3 + elif (chP == 1) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3 + else: + if self.debug: + print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN) + return -1 + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init()) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set 'start single-conversion' bit + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write config register to the ADC + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + + # Wait for the ADC conversion to complete + # The minimum delay depends on the sps: delay >= 1/sps + # We add 0.1ms to be sure + delay = 1.0 / sps + 0.0001 + time.sleep(delay) + + # Read the conversion results + result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2) + if self.ic == self.__IC_ADS1015: + # Shift right 4 bits for the 12-bit ADS1015 and convert to mV + return (((result[0] << 8) | (result[1] & 0xFF)) >> 4) * pga / 2048.0 + else: + # Return a mV value for the ADS1115 + # (Take signed values into account as well) + val = (result[0] << 8) | (result[1]) + if val > 0x7FFF: + return (val - 0xFFFF) * pga / 32768.0 + else: + return ((result[0] << 8) | (result[1])) * pga / 32768.0 + + def readADCDifferential01(self, pga=6144, sps=250): + "Gets a differential ADC reading from channels 0 and 1 in mV\ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see data sheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + return self.readADCDifferential(0, 1, pga, sps) + + def readADCDifferential03(self, pga=6144, sps=250): + "Gets a differential ADC reading from channels 0 and 3 in mV \ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see data sheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + return self.readADCDifferential(0, 3, pga, sps) + + def readADCDifferential13(self, pga=6144, sps=250): + "Gets a differential ADC reading from channels 1 and 3 in mV \ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see data sheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + return self.__readADCDifferential(1, 3, pga, sps) + + def readADCDifferential23(self, pga=6144, sps=250): + "Gets a differential ADC reading from channels 2 and 3 in mV \ + The sample rate for this mode (single-shot) can be used to lower the noise \ + (low sps) or to lower the power consumption (high sps) by duty cycling, \ + see data sheet page 14 for more info. \ + The pga must be given in mV, see page 13 for the supported values." + return self.readADCDifferential(2, 3, pga, sps) + + def startContinuousConversion(self, channel=0, pga=6144, sps=250): + "Starts the continuous conversion mode and returns the first ADC reading \ + in mV from the specified channel. \ + The sps controls the sample rate. \ + The pga must be given in mV, see datasheet page 13 for the supported values. \ + Use getLastConversionResults() to read the next values and \ + stopContinuousConversion() to stop converting." + + # Default to channel 0 with invalid channel, or return -1? + if channel > 3: + if self.debug: + print "ADS1x15: Invalid channel specified: %d" % channel + return -1 + + # Disable comparator, Non-latching, Alert/Rdy active low + # traditional comparator, continuous mode + # The last flag is the only change we need, page 11 datasheet + config = ( + self.__ADS1015_REG_CONFIG_CQUE_NONE + | self.__ADS1015_REG_CONFIG_CLAT_NONLAT + | self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + | self.__ADS1015_REG_CONFIG_CMODE_TRAD + | self.__ADS1015_REG_CONFIG_MODE_CONTIN + ) + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init()) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set the channel to be converted + if channel == 3: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3 + elif channel == 2: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2 + elif channel == 1: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1 + else: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0 + + # Set 'start single-conversion' bit to begin conversions + # No need to change this for continuous mode! + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write config register to the ADC + # Once we write the ADC will convert continously + # we can read the next values using getLastConversionResult + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + + # Wait for the ADC conversion to complete + # The minimum delay depends on the sps: delay >= 1/sps + # We add 0.5ms to be sure + delay = 1.0 / sps + 0.0005 + time.sleep(delay) + + # Read the conversion results + result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2) + if self.ic == self.__IC_ADS1015: + # Shift right 4 bits for the 12-bit ADS1015 and convert to mV + return (((result[0] << 8) | (result[1] & 0xFF)) >> 4) * pga / 2048.0 + else: + # Return a mV value for the ADS1115 + # (Take signed values into account as well) + val = (result[0] << 8) | (result[1]) + if val > 0x7FFF: + return (val - 0xFFFF) * pga / 32768.0 + else: + return ((result[0] << 8) | (result[1])) * pga / 32768.0 + + def startContinuousDifferentialConversion(self, chP=0, chN=1, pga=6144, sps=250): + "Starts the continuous differential conversion mode and returns the first ADC reading \ + in mV as the difference from the specified channels. \ + The sps controls the sample rate. \ + The pga must be given in mV, see datasheet page 13 for the supported values. \ + Use getLastConversionResults() to read the next values and \ + stopContinuousConversion() to stop converting." + + # Disable comparator, Non-latching, Alert/Rdy active low + # traditional comparator, continuous mode + # The last flag is the only change we need, page 11 datasheet + config = ( + self.__ADS1015_REG_CONFIG_CQUE_NONE + | self.__ADS1015_REG_CONFIG_CLAT_NONLAT + | self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + | self.__ADS1015_REG_CONFIG_CMODE_TRAD + | self.__ADS1015_REG_CONFIG_MODE_CONTIN + ) + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init()) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set channels + if (chP == 0) & (chN == 1): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1 + elif (chP == 0) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3 + elif (chP == 2) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3 + elif (chP == 1) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3 + else: + if self.debug: + print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN) + return -1 + + # Set 'start single-conversion' bit to begin conversions + # No need to change this for continuous mode! + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write config register to the ADC + # Once we write the ADC will convert continously + # we can read the next values using getLastConversionResult + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + + # Wait for the ADC conversion to complete + # The minimum delay depends on the sps: delay >= 1/sps + # We add 0.5ms to be sure + delay = 1.0 / sps + 0.0005 + time.sleep(delay) + + # Read the conversion results + result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2) + if self.ic == self.__IC_ADS1015: + # Shift right 4 bits for the 12-bit ADS1015 and convert to mV + return (((result[0] << 8) | (result[1] & 0xFF)) >> 4) * pga / 2048.0 + else: + # Return a mV value for the ADS1115 + # (Take signed values into account as well) + val = (result[0] << 8) | (result[1]) + if val > 0x7FFF: + return (val - 0xFFFF) * pga / 32768.0 + else: + return ((result[0] << 8) | (result[1])) * pga / 32768.0 + + def stopContinuousConversion(self): + "Stops the ADC's conversions when in continuous mode \ + and resets the configuration to its default value." + # Write the default config register to the ADC + # Once we write, the ADC will do a single conversion and + # enter power-off mode. + config = 0x8583 # Page 18 datasheet. + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + return True + + def getLastConversionResults(self): + "Returns the last ADC conversion result in mV" + # Read the conversion results + result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2) + if self.ic == self.__IC_ADS1015: + # Shift right 4 bits for the 12-bit ADS1015 and convert to mV + return (((result[0] << 8) | (result[1] & 0xFF)) >> 4) * self.pga / 2048.0 + else: + # Return a mV value for the ADS1115 + # (Take signed values into account as well) + val = (result[0] << 8) | (result[1]) + if val > 0x7FFF: + return (val - 0xFFFF) * self.pga / 32768.0 + else: + return ((result[0] << 8) | (result[1])) * self.pga / 32768.0 + + def startSingleEndedComparator( + self, + channel, + thresholdHigh, + thresholdLow, + pga=6144, + sps=250, + activeLow=True, + traditionalMode=True, + latching=False, + numReadings=1, + ): + "Starts the comparator mode on the specified channel, see datasheet pg. 15. \ + In traditional mode it alerts (ALERT pin will go low) when voltage exceeds \ + thresholdHigh until it falls below thresholdLow (both given in mV). \ + In window mode (traditionalMode=False) it alerts when voltage doesn't lie\ + between both thresholds.\ + In latching mode the alert will continue until the conversion value is read. \ + numReadings controls how many readings are necessary to trigger an alert: 1, 2 or 4.\ + Use getLastConversionResults() to read the current value (which may differ \ + from the one that triggered the alert) and clear the alert pin in latching mode. \ + This function starts the continuous conversion mode. The sps controls \ + the sample rate and the pga the gain, see datasheet page 13. " + + # With invalid channel return -1 + if channel > 3: + if self.debug: + print "ADS1x15: Invalid channel specified: %d" % channel + return -1 + + # Continuous mode + config = self.__ADS1015_REG_CONFIG_MODE_CONTIN + + if activeLow == False: + config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVHI + else: + config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + + if traditionalMode == False: + config |= self.__ADS1015_REG_CONFIG_CMODE_WINDOW + else: + config |= self.__ADS1015_REG_CONFIG_CMODE_TRAD + + if latching == True: + config |= self.__ADS1015_REG_CONFIG_CLAT_LATCH + else: + config |= self.__ADS1015_REG_CONFIG_CLAT_NONLAT + + if numReadings == 4: + config |= self.__ADS1015_REG_CONFIG_CQUE_4CONV + elif numReadings == 2: + config |= self.__ADS1015_REG_CONFIG_CQUE_2CONV + else: + config |= self.__ADS1015_REG_CONFIG_CQUE_1CONV + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init()) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + if (sps not in self.spsADS1015) & self.debug: + print "ADS1x15: Invalid sps specified: %d, using 1600sps" % sps + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid sps specified: %d, using 250sps" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % pga + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set the channel to be converted + if channel == 3: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3 + elif channel == 2: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2 + elif channel == 1: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1 + else: + config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0 + + # Set 'start single-conversion' bit to begin conversions + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write threshold high and low registers to the ADC + # V_digital = (2^(n-1)-1)/pga*V_analog + if self.ic == self.__IC_ADS1015: + thresholdHighWORD = int(thresholdHigh * (2048.0 / pga)) + else: + thresholdHighWORD = int(thresholdHigh * (32767.0 / pga)) + bytes = [(thresholdHighWORD >> 8) & 0xFF, thresholdHighWORD & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_HITHRESH, bytes) + + if self.ic == self.__IC_ADS1015: + thresholdLowWORD = int(thresholdLow * (2048.0 / pga)) + else: + thresholdLowWORD = int(thresholdLow * (32767.0 / pga)) + bytes = [(thresholdLowWORD >> 8) & 0xFF, thresholdLowWORD & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_LOWTHRESH, bytes) + + # Write config register to the ADC + # Once we write the ADC will convert continously and alert when things happen, + # we can read the converted values using getLastConversionResult + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) + + def startDifferentialComparator( + self, + chP, + chN, + thresholdHigh, + thresholdLow, + pga=6144, + sps=250, + activeLow=True, + traditionalMode=True, + latching=False, + numReadings=1, + ): + "Starts the comparator mode on the specified channel, see datasheet pg. 15. \ + In traditional mode it alerts (ALERT pin will go low) when voltage exceeds \ + thresholdHigh until it falls below thresholdLow (both given in mV). \ + In window mode (traditionalMode=False) it alerts when voltage doesn't lie\ + between both thresholds.\ + In latching mode the alert will continue until the conversion value is read. \ + numReadings controls how many readings are necessary to trigger an alert: 1, 2 or 4.\ + Use getLastConversionResults() to read the current value (which may differ \ + from the one that triggered the alert) and clear the alert pin in latching mode. \ + This function starts the continuous conversion mode. The sps controls \ + the sample rate and the pga the gain, see datasheet page 13. " + + # Continuous mode + config = self.__ADS1015_REG_CONFIG_MODE_CONTIN + + if activeLow == False: + config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVHI + else: + config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW + + if traditionalMode == False: + config |= self.__ADS1015_REG_CONFIG_CMODE_WINDOW + else: + config |= self.__ADS1015_REG_CONFIG_CMODE_TRAD + + if latching == True: + config |= self.__ADS1015_REG_CONFIG_CLAT_LATCH + else: + config |= self.__ADS1015_REG_CONFIG_CLAT_NONLAT + + if numReadings == 4: + config |= self.__ADS1015_REG_CONFIG_CQUE_4CONV + elif numReadings == 2: + config |= self.__ADS1015_REG_CONFIG_CQUE_2CONV + else: + config |= self.__ADS1015_REG_CONFIG_CQUE_1CONV + + # Set sample per seconds, defaults to 250sps + # If sps is in the dictionary (defined in init()) it returns the value of the constant + # othewise it returns the value for 250sps. This saves a lot of if/elif/else code! + if self.ic == self.__IC_ADS1015: + if (sps not in self.spsADS1015) & self.debug: + print "ADS1x15: Invalid sps specified: %d, using 1600sps" % sps + config |= self.spsADS1015.setdefault( + sps, self.__ADS1015_REG_CONFIG_DR_1600SPS + ) + else: + if (sps not in self.spsADS1115) & self.debug: + print "ADS1x15: Invalid sps specified: %d, using 250sps" % sps + config |= self.spsADS1115.setdefault( + sps, self.__ADS1115_REG_CONFIG_DR_250SPS + ) + + # Set PGA/voltage range, defaults to +-6.144V + if (pga not in self.pgaADS1x15) & self.debug: + print "ADS1x15: Invalid pga specified: %d, using 6144mV" % pga + config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V) + self.pga = pga + + # Set channels + if (chP == 0) & (chN == 1): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1 + elif (chP == 0) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3 + elif (chP == 2) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3 + elif (chP == 1) & (chN == 3): + config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3 + else: + if self.debug: + print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN) + return -1 + + # Set 'start single-conversion' bit to begin conversions + config |= self.__ADS1015_REG_CONFIG_OS_SINGLE + + # Write threshold high and low registers to the ADC + # V_digital = (2^(n-1)-1)/pga*V_analog + if self.ic == self.__IC_ADS1015: + thresholdHighWORD = int(thresholdHigh * (2048.0 / pga)) + else: + thresholdHighWORD = int(thresholdHigh * (32767.0 / pga)) + bytes = [(thresholdHighWORD >> 8) & 0xFF, thresholdHighWORD & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_HITHRESH, bytes) + + if self.ic == self.__IC_ADS1015: + thresholdLowWORD = int(thresholdLow * (2048.0 / pga)) + else: + thresholdLowWORD = int(thresholdLow * (32767.0 / pga)) + bytes = [(thresholdLowWORD >> 8) & 0xFF, thresholdLowWORD & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_LOWTHRESH, bytes) + + # Write config register to the ADC + # Once we write the ADC will convert continously and alert when things happen, + # we can read the converted values using getLastConversionResult + bytes = [(config >> 8) & 0xFF, config & 0xFF] + self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes) diff --git a/Python/navio/Navio/adafruit_i2c.py b/Python/navio/Navio/adafruit_i2c.py new file mode 100644 index 00000000..bd377a53 --- /dev/null +++ b/Python/navio/Navio/adafruit_i2c.py @@ -0,0 +1,178 @@ +#!/usr/bin/python +import re +import smbus + +# =========================================================================== +# Adafruit_I2C Class +# =========================================================================== + + +class Adafruit_I2C(object): + @staticmethod + def getPiRevision(): + "Gets the version number of the Raspberry Pi board" + # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History + try: + with open("/proc/cpuinfo", "r") as infile: + for line in infile: + # Match a line of the form "Revision : 0002" while ignoring extra + # info in front of the revsion (like 1000 when the Pi was over-volted). + match = re.match("Revision\s+:\s+.*(\w{4})$", line) + if match and match.group(1) in ["0000", "0002", "0003"]: + # Return revision 1 if revision ends with 0000, 0002 or 0003. + return 1 + elif match: + # Assume revision 2 if revision ends with any other 4 chars. + return 2 + # Couldn't find the revision, assume revision 0 like older code for compatibility. + return 0 + except: + return 0 + + @staticmethod + def getPiI2CBusNumber(): + # Gets the I2C bus number /dev/i2c# + return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 + + def __init__(self, address, busnum=-1, debug=False): + self.address = address + # By default, the correct I2C bus is auto-detected using /proc/cpuinfo + # Alternatively, you can hard-code the bus version below: + # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) + # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) + self.bus = smbus.SMBus( + busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber() + ) + self.debug = debug + + def reverseByteOrder(self, data): + "Reverses the byte order of an int (16-bit) or long (32-bit) value" + # Courtesy Vishal Sapre + byteCount = len(hex(data)[2:].replace("L", "")[::2]) + val = 0 + for i in range(byteCount): + val = (val << 8) | (data & 0xFF) + data >>= 8 + return val + + def errMsg(self): + print "Error accessing 0x%02X: Check your I2C address" % self.address + return -1 + + def write8(self, reg, value): + "Writes an 8-bit value to the specified register/address" + try: + self.bus.write_byte_data(self.address, reg, value) + if self.debug: + print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) + except IOError, err: + return self.errMsg() + + def write16(self, reg, value): + "Writes a 16-bit value to the specified register/address pair" + try: + self.bus.write_word_data(self.address, reg, value) + if self.debug: + print ( + "I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" + % (value, reg, reg + 1) + ) + except IOError, err: + return self.errMsg() + + def writeRaw8(self, value): + "Writes an 8-bit value on the bus" + try: + self.bus.write_byte(self.address, value) + if self.debug: + print "I2C: Wrote 0x%02X" % value + except IOError, err: + return self.errMsg() + + def writeList(self, reg, list): + "Writes an array of bytes using I2C format" + try: + if self.debug: + print "I2C: Writing list to register 0x%02X:" % reg + print list + self.bus.write_i2c_block_data(self.address, reg, list) + except IOError, err: + return self.errMsg() + + def readList(self, reg, length): + "Read a list of bytes from the I2C device" + try: + results = self.bus.read_i2c_block_data(self.address, reg, length) + if self.debug: + print ( + "I2C: Device 0x%02X returned the following from reg 0x%02X" + % (self.address, reg) + ) + print results + return results + except IOError, err: + return self.errMsg() + + def readU8(self, reg): + "Read an unsigned byte from the I2C device" + try: + result = self.bus.read_byte_data(self.address, reg) + if self.debug: + print ( + "I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" + % (self.address, result & 0xFF, reg) + ) + return result + except IOError, err: + return self.errMsg() + + def readS8(self, reg): + "Reads a signed byte from the I2C device" + try: + result = self.bus.read_byte_data(self.address, reg) + if result > 127: + result -= 256 + if self.debug: + print ( + "I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" + % (self.address, result & 0xFF, reg) + ) + return result + except IOError, err: + return self.errMsg() + + def readU16(self, reg, little_endian=True): + "Reads an unsigned 16-bit value from the I2C device" + try: + result = self.bus.read_word_data(self.address, reg) + # Swap bytes if using big endian because read_word_data assumes little + # endian on ARM (little endian) systems. + if not little_endian: + result = ((result << 8) & 0xFF00) + (result >> 8) + if self.debug: + print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % ( + self.address, + result & 0xFFFF, + reg, + ) + return result + except IOError, err: + return self.errMsg() + + def readS16(self, reg, little_endian=True): + "Reads a signed 16-bit value from the I2C device" + try: + result = self.readU16(reg, little_endian) + if result > 32767: + result -= 65536 + return result + except IOError, err: + return self.errMsg() + + +if __name__ == "__main__": + try: + bus = Adafruit_I2C(address=0) + print "Default I2C bus is accessible" + except: + print "Error accessing default I2C bus" diff --git a/Python/navio/Navio/adafruit_pwm_servo_driver.py b/Python/navio/Navio/adafruit_pwm_servo_driver.py new file mode 100644 index 00000000..0e9a3581 --- /dev/null +++ b/Python/navio/Navio/adafruit_pwm_servo_driver.py @@ -0,0 +1,93 @@ +#!/usr/bin/python + +import time +import math +from adafruit_i2c import Adafruit_I2C + +# ============================================================================ +# Adafruit PCA9685 16-Channel PWM Servo Driver +# ============================================================================ + + +class PWM: + # Registers/etc. + __MODE1 = 0x00 + __MODE2 = 0x01 + __SUBADR1 = 0x02 + __SUBADR2 = 0x03 + __SUBADR3 = 0x04 + __PRESCALE = 0xFE + __LED0_ON_L = 0x06 + __LED0_ON_H = 0x07 + __LED0_OFF_L = 0x08 + __LED0_OFF_H = 0x09 + __ALL_LED_ON_L = 0xFA + __ALL_LED_ON_H = 0xFB + __ALL_LED_OFF_L = 0xFC + __ALL_LED_OFF_H = 0xFD + + # Bits + __RESTART = 0x80 + __SLEEP = 0x10 + __ALLCALL = 0x01 + __INVRT = 0x10 + __OUTDRV = 0x04 + + general_call_i2c = Adafruit_I2C(0x00) + + @classmethod + def softwareReset(cls): + "Sends a software reset (SWRST) command to all the servo drivers on the bus" + cls.general_call_i2c.writeRaw8(0x06) # SWRST + + def __init__(self, address=0x40, debug=False): + self.i2c = Adafruit_I2C(address) + self.i2c.debug = debug + self.address = address + self.debug = debug + if self.debug: + print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2" + self.setAllPWM(0, 0) + self.i2c.write8(self.__MODE2, self.__OUTDRV) + self.i2c.write8(self.__MODE1, self.__ALLCALL) + time.sleep(0.005) # wait for oscillator + + mode1 = self.i2c.readU8(self.__MODE1) + mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep) + self.i2c.write8(self.__MODE1, mode1) + time.sleep(0.005) # wait for oscillator + + def setPWMFreq(self, freq): + "Sets the PWM frequency" + prescaleval = 25000000.0 # 25MHz + prescaleval /= 4096.0 # 12-bit + prescaleval /= float(freq) + prescaleval -= 1.0 + if self.debug: + print "Setting PWM frequency to %d Hz" % freq + print "Estimated pre-scale: %d" % prescaleval + prescale = math.floor(prescaleval + 0.5) + if self.debug: + print "Final pre-scale: %d" % prescale + + oldmode = self.i2c.readU8(self.__MODE1) + newmode = (oldmode & 0x7F) | 0x10 # sleep + self.i2c.write8(self.__MODE1, newmode) # go to sleep + self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) + self.i2c.write8(self.__MODE1, oldmode) + time.sleep(0.005) + self.i2c.write8(self.__MODE1, oldmode | 0x80) + + def setPWM(self, channel, on, off): + "Sets a single PWM channel" + self.i2c.write8(self.__LED0_ON_L + 4 * channel, on & 0xFF) + self.i2c.write8(self.__LED0_ON_H + 4 * channel, on >> 8) + self.i2c.write8(self.__LED0_OFF_L + 4 * channel, off & 0xFF) + self.i2c.write8(self.__LED0_OFF_H + 4 * channel, off >> 8) + + def setAllPWM(self, on, off): + "Sets a all PWM channels" + self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF) + self.i2c.write8(self.__ALL_LED_ON_H, on >> 8) + self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF) + self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8) diff --git a/Python/navio/Navio/gpio.py b/Python/navio/Navio/gpio.py new file mode 100644 index 00000000..e1f954c8 --- /dev/null +++ b/Python/navio/Navio/gpio.py @@ -0,0 +1,43 @@ +import os + + +class Pin: + """Minimal wrapper for Pins. To be deprecated soon""" + + def __get_direction(self): + with open("/sys/class/gpio/gpio%d/direction" % self.pin, "r") as direction_file: + direction = direction_file.read() + return direction + + def __set_direction(self, direction): + with open("/sys/class/gpio/gpio%d/direction" % self.pin, "w") as direction_file: + direction_file.write(direction) + + def __init__(self, pin): + self.pin = pin + try: + with open("/sys/class/gpio/export", "a") as sysfs_export: + sysfs_export.write("%s" % pin) + except IOError: + # already exported. nothing to do + pass + + self.direction = self.__get_direction() + + def write(self, value): + if self.direction != "out": + self.__set_direction("out") + + with open("/sys/class/gpio/gpio%d/value" % self.pin, "w") as value_file: + value_file.write(str(value)) + + def read(self): + with open("/sys/class/gpio/gpio%d/value" % self.pin, "r") as value_file: + return int(value_file.read()) + + +if __name__ == "__main__": + pin = Pin(27) + pin.write(0) + + assert pin.read() == 0 diff --git a/Python/navio/adc.py b/Python/navio/Navio2/ADC.py similarity index 69% rename from Python/navio/adc.py rename to Python/navio/Navio2/ADC.py index c680e954..ca612ae6 100644 --- a/Python/navio/adc.py +++ b/Python/navio/Navio2/ADC.py @@ -1,6 +1,7 @@ import os.path -class ADC(): + +class ADC: SYSFS_ADC_PATH_BASE = "/sys/kernel/rcio/adc/" channel_count = 6 channels = [] @@ -9,10 +10,12 @@ def __init__(self): if not os.path.exists(self.SYSFS_ADC_PATH_BASE): raise OSError("rcio_adc module wasn't loaded") - self.channels = [open(self.SYSFS_ADC_PATH_BASE + "ch{}".format(channel), "r") for channel in range(self.channel_count)] - + self.channels = [ + open(self.SYSFS_ADC_PATH_BASE + "ch{}".format(channel), "r") + for channel in range(self.channel_count) + ] + def read(self, ch): value = self.channels[ch].read() position = self.channels[ch].seek(0, 0) return float(value[:-1]) - diff --git a/Python/navio/Navio2/Led.py b/Python/navio/Navio2/Led.py new file mode 100644 index 00000000..e25adc31 --- /dev/null +++ b/Python/navio/Navio2/Led.py @@ -0,0 +1,38 @@ +ON = 0 +OFF = 1 + + +class Pin: + def __init__(self, folder_name): + self.pin = folder_name + + def write(self, value): + with open("/sys/class/leds/%s/brightness" % self.pin, "w") as value_file: + value_file.write(str(value)) + + +class Led: + def __init__(self): + self.ledR = Pin("rgb_led0") + self.ledB = Pin("rgb_led1") + self.ledG = Pin("rgb_led2") + + self.ledR.write(OFF) + self.ledG.write(OFF) + self.ledB.write(OFF) + + def setColor(self, color): + self.ledR.write(self.gamma[color][0]) + self.ledG.write(self.gamma[color][1]) + self.ledB.write(self.gamma[color][2]) + + gamma = { + "Black": (OFF, OFF, OFF), + "Red": (ON, OFF, OFF), + "Green": (OFF, ON, OFF), + "Blue": (OFF, OFF, ON), + "Cyan": (OFF, ON, ON), + "Magenta": (ON, OFF, ON), + "Yellow": (ON, ON, OFF), + "White": (ON, ON, ON), + } diff --git a/Python/navio/Navio2/RCInput.py b/Python/navio/Navio2/RCInput.py new file mode 100644 index 00000000..06eefce0 --- /dev/null +++ b/Python/navio/Navio2/RCInput.py @@ -0,0 +1,20 @@ +class RCInput: + CHANNEL_COUNT = 14 + channels = [] + + def __init__(self): + for i in range(0, self.CHANNEL_COUNT): + try: + f = open("/sys/kernel/rcio/rcin/ch%d" % i, "r") + self.channels.append(f) + except: + print("Can't open file /sys/kernel/rcio/rcin/ch%d" % i) + + def read(self, ch): + try: + value = self.channels[ch].read() + position = self.channels[ch].seek(0, 0) + return value[:-1] + except IndexError: + print("Channel number too large") + exit(1) diff --git a/Python/navio/pwm.py b/Python/navio/Navio2/RCOutput.py similarity index 92% rename from Python/navio/pwm.py rename to Python/navio/Navio2/RCOutput.py index cf0c9f23..f5bce9fd 100644 --- a/Python/navio/pwm.py +++ b/Python/navio/Navio2/RCOutput.py @@ -1,6 +1,7 @@ import os.path -class PWM(): + +class RCOutput: SYSFS_PWM_PATH_BASE = "/sys/class/pwm/pwmchip0/" SYSFS_PWM_EXPORT_PATH = "/sys/class/pwm/pwmchip0/export" SYSFS_PWM_UNEXPORT_PATH = "/sys/class/pwm/pwmchip0/unexport" @@ -49,14 +50,14 @@ def set_period(self, freq): if not self.is_initialized: raise RuntimeError("PWM not initialized. Call initialize first") - period_ns = int(1e9/freq) - with open(self.channel_path + "period", "w") as pwm_period: + period_ns = int(1e9 / freq) + with open(self.channel_path + "period", "w") as pwm_period: pwm_period.write(str(period_ns)) def set_duty_cycle(self, period): if not self.is_initialized: raise RuntimeError("PWM not initialized. Call initialize first") - period_ns = int(period*1e6) + period_ns = int(period * 1e6) with open(self.channel_path + "duty_cycle", "w") as pwm_duty: pwm_duty.write(str(period_ns)) diff --git a/Python/navio/Navio2/__init__.py b/Python/navio/Navio2/__init__.py new file mode 100644 index 00000000..e7ffeab4 --- /dev/null +++ b/Python/navio/Navio2/__init__.py @@ -0,0 +1,5 @@ +from .lsm9ds1 import LSM9DS1 +from .Led import Led +from .RCOutput import RCOutput +from .RCInput import RCInput +from .ADC import ADC diff --git a/Python/navio/lsm9ds1.py b/Python/navio/Navio2/lsm9ds1.py similarity index 67% rename from Python/navio/lsm9ds1.py rename to Python/navio/Navio2/lsm9ds1.py index 4c8e867d..d3c212f1 100644 --- a/Python/navio/lsm9ds1.py +++ b/Python/navio/Navio2/lsm9ds1.py @@ -29,6 +29,7 @@ import array import struct + class LSM9DS1: __DEVICE_ACC_GYRO = 3 @@ -40,7 +41,7 @@ class LSM9DS1: # who am I values __WHO_AM_I_ACC_GYRO = 0x68 __WHO_AM_I_MAG = 0x3D - + # Accelerometer and Gyroscope registers __LSM9DS1XG_ACT_THS = 0x04 __LSM9DS1XG_ACT_DUR = 0x05 @@ -52,7 +53,7 @@ class LSM9DS1: __LSM9DS1XG_REFERENCE_G = 0x0B __LSM9DS1XG_INT1_CTRL = 0x0C __LSM9DS1XG_INT2_CTRL = 0x0D - __LSM9DS1XG_WHO_AM_I = 0x0F # should return = 0x68 + __LSM9DS1XG_WHO_AM_I = 0x0F # should return = 0x68 __LSM9DS1XG_CTRL_REG1_G = 0x10 __LSM9DS1XG_CTRL_REG2_G = 0x11 __LSM9DS1XG_CTRL_REG3_G = 0x12 @@ -91,7 +92,7 @@ class LSM9DS1: __LSM9DS1XG_INT_GEN_THS_ZH_G = 0x35 __LSM9DS1XG_INT_GEN_THS_ZL_G = 0x36 __LSM9DS1XG_INT_GEN_DUR_G = 0x37 - + # Magnetometer registers __LSM9DS1M_OFFSET_X_REG_L_M = 0x05 __LSM9DS1M_OFFSET_X_REG_H_M = 0x06 @@ -99,7 +100,7 @@ class LSM9DS1: __LSM9DS1M_OFFSET_Y_REG_H_M = 0x08 __LSM9DS1M_OFFSET_Z_REG_L_M = 0x09 __LSM9DS1M_OFFSET_Z_REG_H_M = 0x0A - __LSM9DS1M_WHO_AM_I = 0x0F # should return = 0x3D + __LSM9DS1M_WHO_AM_I = 0x0F # should return = 0x3D __LSM9DS1M_CTRL_REG1_M = 0x20 __LSM9DS1M_CTRL_REG2_M = 0x21 __LSM9DS1M_CTRL_REG3_M = 0x22 @@ -116,7 +117,7 @@ class LSM9DS1: __LSM9DS1M_INT_SRC_M = 0x31 __LSM9DS1M_INT_THS_L_M = 0x32 __LSM9DS1M_INT_THS_H_M = 0x33 - + # Configuration bits Accelerometer and Gyroscope __BITS_XEN_G = 0x08 __BITS_YEN_G = 0x10 @@ -145,7 +146,7 @@ class LSM9DS1: __BITS_FS_XL_4G = 0x10 __BITS_FS_XL_8G = 0x18 __BITS_FS_XL_16G = 0x08 - + # Configuration bits Magnetometer __BITS_TEMP_COMP = 0x80 __BITS_OM_LOW = 0x00 @@ -176,7 +177,7 @@ class LSM9DS1: __READ_FLAG = 0x80 __MULTIPLE_READ = 0x40 - def __init__(self, spi_bus_number = 0): + def __init__(self, spi_bus_number=0): self.bus = spidev.SpiDev() self.spi_bus_number = spi_bus_number self.gyro_scale = None @@ -193,8 +194,8 @@ def bus_open(self, dev_number): def testConnection(self): responseXG = self.readReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_WHO_AM_I) - responseM = self.readReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_WHO_AM_I) - if (responseXG == self.__WHO_AM_I_ACC_GYRO and responseM == self.__WHO_AM_I_MAG): + responseM = self.readReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_WHO_AM_I) + if responseXG == self.__WHO_AM_I_ACC_GYRO and responseM == self.__WHO_AM_I_MAG: return True return False @@ -217,105 +218,162 @@ def readRegs(self, dev_number, reg_address, length): tx = [0] * (length + 1) tx[0] = reg_address | self.__READ_FLAG - if dev_number==self.__DEVICE_MAGNETOMETER: + if dev_number == self.__DEVICE_MAGNETOMETER: tx[0] |= self.__MULTIPLE_READ rx = self.bus.xfer2(tx) self.bus.close() - return rx[1:len(rx)] + return rx[1 : len(rx)] def initialize(self): - #--------Accelerometer and Gyroscope--------- + # --------Accelerometer and Gyroscope--------- # enable the 3-axes of the gyroscope - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG4, self.__BITS_XEN_G | self.__BITS_YEN_G | self.__BITS_ZEN_G) + self.writeReg( + self.__DEVICE_ACC_GYRO, + self.__LSM9DS1XG_CTRL_REG4, + self.__BITS_XEN_G | self.__BITS_YEN_G | self.__BITS_ZEN_G, + ) # configure the gyroscope - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG1_G, self.__BITS_ODR_G_952HZ | self.__BITS_FS_G_2000DPS) + self.writeReg( + self.__DEVICE_ACC_GYRO, + self.__LSM9DS1XG_CTRL_REG1_G, + self.__BITS_ODR_G_952HZ | self.__BITS_FS_G_2000DPS, + ) time.sleep(0.1) # enable the three axes of the accelerometer - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG5_XL, self.__BITS_XEN_XL | self.__BITS_YEN_XL | self.__BITS_ZEN_XL) + self.writeReg( + self.__DEVICE_ACC_GYRO, + self.__LSM9DS1XG_CTRL_REG5_XL, + self.__BITS_XEN_XL | self.__BITS_YEN_XL | self.__BITS_ZEN_XL, + ) # configure the accelerometer-specify bandwidth selection with Abw - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG6_XL, self.__BITS_ODR_XL_952HZ | self.__BITS_FS_XL_16G) + self.writeReg( + self.__DEVICE_ACC_GYRO, + self.__LSM9DS1XG_CTRL_REG6_XL, + self.__BITS_ODR_XL_952HZ | self.__BITS_FS_XL_16G, + ) time.sleep(0.1) - #------------Magnetometer---------------- - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG1_M, self.__BITS_TEMP_COMP | self.__BITS_OM_HIGH | self.__BITS_ODR_M_80HZ) - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG2_M, self.__BITS_FS_M_16Gs) + # ------------Magnetometer---------------- + self.writeReg( + self.__DEVICE_MAGNETOMETER, + self.__LSM9DS1M_CTRL_REG1_M, + self.__BITS_TEMP_COMP | self.__BITS_OM_HIGH | self.__BITS_ODR_M_80HZ, + ) + self.writeReg( + self.__DEVICE_MAGNETOMETER, + self.__LSM9DS1M_CTRL_REG2_M, + self.__BITS_FS_M_16Gs, + ) # continuous conversion mode - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG3_M, self.__BITS_MD_CONTINUOUS) - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG4_M, self.__BITS_OMZ_HIGH) - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG5_M, 0x00 ) + self.writeReg( + self.__DEVICE_MAGNETOMETER, + self.__LSM9DS1M_CTRL_REG3_M, + self.__BITS_MD_CONTINUOUS, + ) + self.writeReg( + self.__DEVICE_MAGNETOMETER, + self.__LSM9DS1M_CTRL_REG4_M, + self.__BITS_OMZ_HIGH, + ) + self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG5_M, 0x00) time.sleep(0.1) self.set_gyro_scale(self.__BITS_FS_G_2000DPS) self.set_acc_scale(self.__BITS_FS_XL_16G) self.set_mag_scale(self.__BITS_FS_M_16Gs) - def set_gyro_scale(self, scale): - reg = self.__BITS_FS_G_MASK & self.readReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG1_G) - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG1_G,reg | scale) - if (scale == self.__BITS_FS_G_245DPS): + reg = self.__BITS_FS_G_MASK & self.readReg( + self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG1_G + ) + self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG1_G, reg | scale) + if scale == self.__BITS_FS_G_245DPS: self.gyro_scale = 0.00875 - if (scale == self.__BITS_FS_G_500DPS): + if scale == self.__BITS_FS_G_500DPS: self.gyro_scale = 0.0175 - if (scale == self.__BITS_FS_G_2000DPS): + if scale == self.__BITS_FS_G_2000DPS: self.gyro_scale = 0.07 def set_acc_scale(self, scale): - reg = self.__BITS_FS_XL_MASK & self.readReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG6_XL) - self.writeReg(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG6_XL, reg | scale) - if (scale == self.__BITS_FS_XL_2G): + reg = self.__BITS_FS_XL_MASK & self.readReg( + self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG6_XL + ) + self.writeReg( + self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_CTRL_REG6_XL, reg | scale + ) + if scale == self.__BITS_FS_XL_2G: self.acc_scale = 0.000061 - if (scale == self.__BITS_FS_XL_4G): + if scale == self.__BITS_FS_XL_4G: self.acc_scale = 0.000122 - if (scale == self.__BITS_FS_XL_8G): + if scale == self.__BITS_FS_XL_8G: self.acc_scale = 0.000244 - if (scale == self.__BITS_FS_XL_16G): + if scale == self.__BITS_FS_XL_16G: self.acc_scale = 0.000732 def set_mag_scale(self, scale): - reg = self.__BITS_FS_M_MASK & self.readReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG2_M) - self.writeReg(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG2_M, reg | scale) - if (scale == self.__BITS_FS_M_4Gs): + reg = self.__BITS_FS_M_MASK & self.readReg( + self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG2_M + ) + self.writeReg( + self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_CTRL_REG2_M, reg | scale + ) + if scale == self.__BITS_FS_M_4Gs: self.mag_scale = 0.00014 - if (scale == self.__BITS_FS_M_8Gs): + if scale == self.__BITS_FS_M_8Gs: self.mag_scale = 0.00029 - if (scale == self.__BITS_FS_M_12Gs): + if scale == self.__BITS_FS_M_12Gs: self.mag_scale = 0.00043 - if (scale == self.__BITS_FS_M_16Gs): + if scale == self.__BITS_FS_M_16Gs: self.mag_scale = 0.00058 def read_acc(self): # Read accelerometer response = self.readRegs(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_OUT_X_L_XL, 6) for i in xrange(3): - self.accelerometer_data[i] = self.G_SI * (self.byte_to_float_le(response[2*i:2*i+2]) * self.acc_scale) + self.accelerometer_data[i] = self.G_SI * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.acc_scale + ) - self.accelerometer_data = [-self.accelerometer_data[1], -self.accelerometer_data[0], self.accelerometer_data[2]] + self.accelerometer_data = [ + -self.accelerometer_data[1], + -self.accelerometer_data[0], + self.accelerometer_data[2], + ] def read_gyro(self): # Read gyroscope response = self.readRegs(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_OUT_X_L_G, 6) for i in xrange(3): - self.gyroscope_data[i] = (self.PI / 180.0) * (self.byte_to_float_le(response[2*i:2*i+2]) * self.gyro_scale) + self.gyroscope_data[i] = (self.PI / 180.0) * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.gyro_scale + ) - self.gyroscope_data = [-self.gyroscope_data[1], -self.gyroscope_data[0], self.gyroscope_data[2]] + self.gyroscope_data = [ + -self.gyroscope_data[1], + -self.gyroscope_data[0], + self.gyroscope_data[2], + ] def read_mag(self): # Read magnetometer - response = self.readRegs(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_OUT_X_L_M, 6) + response = self.readRegs( + self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_OUT_X_L_M, 6 + ) for i in xrange(3): - self.magnetometer_data[i] = 100.0 * (self.byte_to_float_le(response[2*i:2*i+2]) * self.mag_scale) + self.magnetometer_data[i] = 100.0 * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.mag_scale + ) - self.magnetometer_data[1] *=-1 - self.magnetometer_data[2] *=-1 + self.magnetometer_data[1] *= -1 + self.magnetometer_data[2] *= -1 def read_temp(self): # Read temperature response = self.readRegs(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_OUT_TEMP_L, 2) - self.temperature = self.byte_to_float_le(response) / 256.0 + 25.0 + self.temperature = self.byte_to_float_le(response) / 256.0 + 25.0 def read_all(self): # Read temperature @@ -325,17 +383,25 @@ def read_all(self): # Read accelerometer response = self.readRegs(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_OUT_X_L_XL, 6) for i in xrange(3): - self.accelerometer_data[i] = self.G_SI * (self.byte_to_float_le(response[2*i:2*i+2]) * self.acc_scale) + self.accelerometer_data[i] = self.G_SI * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.acc_scale + ) # Read gyroscope response = self.readRegs(self.__DEVICE_ACC_GYRO, self.__LSM9DS1XG_OUT_X_L_G, 6) for i in xrange(3): - self.gyroscope_data[i] = (self.PI / 180.0) * (self.byte_to_float_le(response[2*i:2*i+2]) * self.gyro_scale) + self.gyroscope_data[i] = (self.PI / 180.0) * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.gyro_scale + ) # Read magnetometer - response = self.readRegs(self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_OUT_X_L_M, 6) + response = self.readRegs( + self.__DEVICE_MAGNETOMETER, self.__LSM9DS1M_OUT_X_L_M, 6 + ) for i in xrange(3): - self.magnetometer_data[i] = 100.0 * (self.byte_to_float_le(response[2*i:2*i+2]) * self.mag_scale) + self.magnetometer_data[i] = 100.0 * ( + self.byte_to_float_le(response[2 * i : 2 * i + 2]) * self.mag_scale + ) # Change rotation of LSM9DS1 like in MPU-9250 self.rotate() @@ -359,19 +425,26 @@ def getMotion6(self): def byte_to_float(self, input_buffer): byte_array = array.array("B", input_buffer) - signed_16_bit_int, = struct.unpack(">h", byte_array) + (signed_16_bit_int,) = struct.unpack(">h", byte_array) return float(signed_16_bit_int) def byte_to_float_le(self, input_buffer): byte_array = array.array("B", input_buffer) - signed_16_bit_int, = struct.unpack(" X axis -# 1 -> Y axis -# 2 -> Z axis -# ----------------------------------------------------------------------------------------------- - - def read_acc(self): - response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 6) - - for i in range(0, 3): - data = self.byte_to_float(response[i*2:i*2+2]) - self.accelerometer_data[i] = self.G_SI*data/self.acc_divider - -# ----------------------------------------------------------------------------------------------- -# READ GYROSCOPE -# usage: call this function to read gyroscope data. Axis represents selected axis: -# 0 -> X axis -# 1 -> Y axis -# 2 -> Z axis -# ----------------------------------------------------------------------------------------------- - - def read_gyro(self): - response = self.ReadRegs(self.__MPUREG_GYRO_XOUT_H, 6) - - for i in range(0, 3): - data = self.byte_to_float(response[i*2:i*2+2]) - self.gyroscope_data[i] = (self.PI/180)*data/self.gyro_divider - -# ----------------------------------------------------------------------------------------------- -# READ TEMPERATURE -# usage: call this function to read temperature data. -# returns the value in Celsius -# ----------------------------------------------------------------------------------------------- - - def read_temp(self): - response = self.ReadRegs(self.__MPUREG_TEMP_OUT_H, 2) - - #temp = response[0]*256.0 + response[1] - temp = self.byte_to_float(response) - self.temperature = (temp/340.0)+36.53 - -# ----------------------------------------------------------------------------------------------- - - def AK8963_whoami(self): - self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) #Set the I2C slave addres of AK8963 and set for read. - self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_WIA) #I2C slave 0 register address from where to begin data transfer - self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) #Read 1 byte from the magnetometer - - #self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) # Enable I2C and set bytes - time.sleep(0.01) - - return self.ReadReg(self.__MPUREG_EXT_SENS_DATA_00) # Read I2C - -# ----------------------------------------------------------------------------------------------- - - def calib_mag(self): - # Set the I2C slave addres of AK8963 and set for read. - self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) - # I2C slave 0 register address from where to begin data transfer - self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_ASAX) - # Read 3 bytes from the magnetometer - self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x83) - - time.sleep(0.01) - - response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 3) - - for i in range(0, 3): - self.magnetometer_ASA[i] = ((float(response[i]) - 128)/256 + 1) * self.__Magnetometer_Sensitivity_Scale_Factor - -# ----------------------------------------------------------------------------------------------- - - def read_mag(self): - # Set the I2C slave addres of AK8963 and set for read. - self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) - # I2C slave 0 register address from where to begin data transfer - self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) - # Read 6 bytes from the magnetometer - self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) - - time.sleep(0.01) - - response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 7) - #must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. - for i in range(0, 3): - data = self.byte_to_float_le(response[i*2:i*2+2]) - self.magnetometer_data[i] = data * self.magnetometer_ASA[i] - -# ----------------------------------------------------------------------------------------------- - - def read_all(self): - # Send I2C command at first - # Set the I2C slave addres of AK8963 and set for read. - self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) - # I2C slave 0 register address from where ; //Read 7 bytes from the magnetometerto begin data transfer - self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL) - # Read 7 bytes from the magnetometer - self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87) - # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. - - # time.sleep(0.001) - response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 21); - - # Get Accelerometer values - for i in range(0, 3): - data = self.byte_to_float(response[i*2:i*2+2]) - self.accelerometer_data[i] = self.G_SI*data/self.acc_divider - - # Get temperature - i = 3 - temp = self.byte_to_float(response[i*2:i*2+2]) - self.temperature = (temp/340.0)+36.53 - - # Get gyroscope values - for i in range(4, 7): - data = self.byte_to_float(response[i*2:i*2+2]) - self.gyroscope_data[i-4] =(self.PI/180)*data/self.gyro_divider - - # Get magnetometer values - for i in range(7, 10): - data = self.byte_to_float_le(response[i*2:i*2+2]) - self.magnetometer_data[i-7] = data * self.magnetometer_ASA[i-7] - -# ----------------------------------------------------------------------------------------------- -# GET VALUES -# usage: call this functions to read and get values -# returns accel, gyro and mag values -# ----------------------------------------------------------------------------------------------- - - def getMotion9(self): - self.read_all() - m9a = self.accelerometer_data - m9g = self.gyroscope_data - m9m = self.magnetometer_data - - return m9a, m9g, m9m - -# ----------------------------------------------------------------------------------------------- - - def getMotion6(self): - self.read_acc() - self.read_gyro() - - m6a = self.accelerometer_data - m6g = self.gyroscope_data - - return m6a, m6g - -# ----------------------------------------------------------------------------------------------- - - def byte_to_float(self, input_buffer): - byte_array = array.array("B", input_buffer) - signed_16_bit_int, = struct.unpack(">h", byte_array) - return float(signed_16_bit_int) - -# ----------------------------------------------------------------------------------------------- - - def byte_to_float_le(self, input_buffer): - byte_array = array.array("B", input_buffer) - signed_16_bit_int, = struct.unpack("= 2000): - T2 = 0 - OFF2 = 0 - SENS2 = 0 - elif (self.TEMP < 2000): - T2 = dT * dT / 2**31 - OFF2 = 5 * ((self.TEMP - 2000) ** 2) / 2 - SENS2 = OFF2 / 2 - elif (self.TEMP < -1500): - OFF2 = OFF2 + 7 * ((self.TEMP + 1500) ** 2) - SENS2 = SENS2 + 11 * (self.TEMP + 1500) ** 2 / 2 - - self.TEMP = self.TEMP - T2 - OFF = OFF - OFF2 - SENS = SENS - SENS2 - - self.PRES = (self.D1 * SENS / 2**21 - OFF) / 2**15 - - self.TEMP = self.TEMP / 100 # Temperature updated - self.PRES = self.PRES / 100 # Pressure updated - - def returnPressure(self): - return self.PRES - - def returnTemperature(self): - return self.TEMP - - def update(self): - self.refreshPressure() - time.sleep(0.01) # Waiting for pressure data ready - self.readPressure() - - self.refreshTemperature() - time.sleep(0.01) # Waiting for temperature data ready - self.readTemperature() - - self.calculatePressureAndTemperature() - - def test(self): - self.initialize() - self.update() - is_pressure_valid = 1000 <= self.PRES <= 1050 - is_temp_valid = -40 <= self.TEMP <= 80 - - return is_pressure_valid and is_temp_valid diff --git a/Python/navio/rcinput.py b/Python/navio/rcinput.py deleted file mode 100644 index ede29d31..00000000 --- a/Python/navio/rcinput.py +++ /dev/null @@ -1,17 +0,0 @@ -class RCInput(): - CHANNEL_COUNT = 14 - channels = [] - - def __init__(self): - for i in range(0, self.CHANNEL_COUNT): - try: - f = open("/sys/kernel/rcio/rcin/ch%d" % i, "r") - self.channels.append(f) - except: - print ("Can't open file /sys/kernel/rcio/rcin/ch%d" % i) - - def read(self, ch): - value = self.channels[ch].read() - position = self.channels[ch].seek(0, 0) - return value[:-1] - diff --git a/Python/navio/ublox.py b/Python/navio/ublox.py deleted file mode 100644 index 649f16ef..00000000 --- a/Python/navio/ublox.py +++ /dev/null @@ -1,998 +0,0 @@ -#!/usr/bin/env python -''' -UBlox binary protocol handling - -Copyright Andrew Tridgell, October 2012 -Released under GNU GPL version 3 or later -''' - -import struct -from datetime import datetime -import time, os -import sys - -# specify Python version -if sys.version_info[0] < 3: # we're on python 2.x.x - PYTHON_VERSION = 2 -else: - PYTHON_VERSION = 3 - -# protocol constants -PREAMBLE1 = 0xb5 -PREAMBLE2 = 0x62 - -# message classes -CLASS_NAV = 0x01 -CLASS_RXM = 0x02 -CLASS_INF = 0x04 -CLASS_ACK = 0x05 -CLASS_CFG = 0x06 -CLASS_MON = 0x0A -CLASS_AID = 0x0B -CLASS_TIM = 0x0D -CLASS_ESF = 0x10 - -# ACK messages -MSG_ACK_NACK = 0x00 -MSG_ACK_ACK = 0x01 - -# NAV messages -MSG_NAV_POSECEF = 0x1 -MSG_NAV_POSLLH = 0x2 -MSG_NAV_STATUS = 0x3 -MSG_NAV_DOP = 0x4 -MSG_NAV_SOL = 0x6 -MSG_NAV_POSUTM = 0x8 -MSG_NAV_VELNED = 0x12 -MSG_NAV_VELECEF = 0x11 -MSG_NAV_TIMEGPS = 0x20 -MSG_NAV_TIMEUTC = 0x21 -MSG_NAV_CLOCK = 0x22 -MSG_NAV_SVINFO = 0x30 -MSG_NAV_AOPSTATUS = 0x60 -MSG_NAV_DGPS = 0x31 -MSG_NAV_DOP = 0x04 -MSG_NAV_EKFSTATUS = 0x40 -MSG_NAV_SBAS = 0x32 -MSG_NAV_SOL = 0x06 -MSG_NAV_PVT = 0x07 - -# RXM messages -MSG_RXM_RAW = 0x10 -MSG_RXM_SFRB = 0x11 -MSG_RXM_SVSI = 0x20 -MSG_RXM_EPH = 0x31 -MSG_RXM_ALM = 0x30 -MSG_RXM_PMREQ = 0x41 - -# AID messages -MSG_AID_ALM = 0x30 -MSG_AID_EPH = 0x31 -MSG_AID_ALPSRV = 0x32 -MSG_AID_AOP = 0x33 -MSG_AID_DATA = 0x10 -MSG_AID_ALP = 0x50 -MSG_AID_DATA = 0x10 -MSG_AID_HUI = 0x02 -MSG_AID_INI = 0x01 -MSG_AID_REQ = 0x00 - -# CFG messages -MSG_CFG_PRT = 0x00 -MSG_CFG_ANT = 0x13 -MSG_CFG_DAT = 0x06 -MSG_CFG_EKF = 0x12 -MSG_CFG_ESFGWT = 0x29 -MSG_CFG_CFG = 0x09 -MSG_CFG_USB = 0x1b -MSG_CFG_RATE = 0x08 -MSG_CFG_SET_RATE = 0x01 -MSG_CFG_NAV5 = 0x24 -MSG_CFG_FXN = 0x0E -MSG_CFG_INF = 0x02 -MSG_CFG_ITFM = 0x39 -MSG_CFG_MSG = 0x01 -MSG_CFG_NAVX5 = 0x23 -MSG_CFG_NMEA = 0x17 -MSG_CFG_NVS = 0x22 -MSG_CFG_PM2 = 0x3B -MSG_CFG_PM = 0x32 -MSG_CFG_RINV = 0x34 -MSG_CFG_RST = 0x04 -MSG_CFG_RXM = 0x11 -MSG_CFG_SBAS = 0x16 -MSG_CFG_TMODE2 = 0x3D -MSG_CFG_TMODE = 0x1D -MSG_CFG_TPS = 0x31 -MSG_CFG_TP = 0x07 -MSG_CFG_GNSS = 0x3E - -# ESF messages -MSG_ESF_MEAS = 0x02 -MSG_ESF_STATUS = 0x10 - -# INF messages -MSG_INF_DEBUG = 0x04 -MSG_INF_ERROR = 0x00 -MSG_INF_NOTICE = 0x02 -MSG_INF_TEST = 0x03 -MSG_INF_WARNING= 0x01 - -# MON messages -MSG_MON_SCHD = 0x01 -MSG_MON_HW = 0x09 -MSG_MON_HW2 = 0x0B -MSG_MON_IO = 0x02 -MSG_MON_MSGPP = 0x06 -MSG_MON_RXBUF = 0x07 -MSG_MON_RXR = 0x21 -MSG_MON_TXBUF = 0x08 -MSG_MON_VER = 0x04 - -# TIM messages -MSG_TIM_TP = 0x01 -MSG_TIM_TM2 = 0x03 -MSG_TIM_SVIN = 0x04 -MSG_TIM_VRFY = 0x06 - -# port IDs -PORT_DDC =0 -PORT_SERIAL1=1 -PORT_SERIAL2=2 -PORT_USB =3 -PORT_SPI =4 - -# dynamic models -DYNAMIC_MODEL_PORTABLE = 0 -DYNAMIC_MODEL_STATIONARY = 2 -DYNAMIC_MODEL_PEDESTRIAN = 3 -DYNAMIC_MODEL_AUTOMOTIVE = 4 -DYNAMIC_MODEL_SEA = 5 -DYNAMIC_MODEL_AIRBORNE1G = 6 -DYNAMIC_MODEL_AIRBORNE2G = 7 -DYNAMIC_MODEL_AIRBORNE4G = 8 - -#reset items -RESET_HOT = 0 -RESET_WARM = 1 -RESET_COLD = 0xFFFF - -RESET_HW = 0 -RESET_SW = 1 -RESET_SW_GPS = 2 -RESET_HW_GRACEFUL = 4 -RESET_GPS_STOP = 8 -RESET_GPS_START = 9 - -class UBloxError(Exception): - '''Ublox error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = msg - -class UBloxAttrDict(dict): - '''allow dictionary members as attributes''' - def __init__(self): - dict.__init__(self) - - def __getattr__(self, name): - try: - return self.__getitem__(name) - except KeyError: - raise AttributeError(name) - - def __setattr__(self, name, value): - if self.__dict__.has_key(name): - # allow set on normal attributes - dict.__setattr__(self, name, value) - else: - self.__setitem__(name, value) - -def ArrayParse(field): - '''parse an array descriptor''' - arridx = field.find('[') - if arridx == -1: - return (field, -1) - alen = int(field[arridx+1:-1]) - fieldname = field[:arridx] - return (fieldname, alen) - -class UBloxDescriptor: - '''class used to describe the layout of a UBlox message''' - def __init__(self, name, msg_format, fields=[], count_field=None, format2=None, fields2=None): - self.name = name - self.msg_format = msg_format - self.fields = fields - self.count_field = count_field - self.format2 = format2 - self.fields2 = fields2 - - def getf(self, fmt, buf, size): - f = list(struct.unpack(fmt, buf[:size])) - return f - - def unpack(self, msg): - '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg''' - msg._fields = {} - - # unpack main message blocks. A comm - formats = self.msg_format.split(',') - buf = msg._buf[6:-2] - count = 0 - msg._recs = [] - fields = self.fields[:] - - for fmt in formats: - size1 = struct.calcsize(fmt) - if size1 > len(buf): - raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf))) - f1 = self.getf(fmt, buf, size1) - - i = 0 - while i < len(f1): - field = fields.pop(0) - (fieldname, alen) = ArrayParse(field) - if alen == -1: - msg._fields[fieldname] = f1[i] - if self.count_field == fieldname: - count = int(f1[i]) - i += 1 - else: - msg._fields[fieldname] = [0]*alen - for a in range(alen): - msg._fields[fieldname][a] = f1[i] - i += 1 - buf = buf[size1:] - if len(buf) == 0: - break - - if self.count_field == '_remaining': - count = len(buf) / struct.calcsize(self.format2) - - if count == 0: - msg._unpacked = True - if len(buf) != 0: - raise UBloxError("EXTRA_BYTES=%u" % len(buf)) - return - - size2 = struct.calcsize(self.format2) - for c in range(count): - r = UBloxAttrDict() - if size2 > len(buf): - raise UBloxError("INVALID_SIZE=%u, " % len(buf)) - f2 = self.getf(self.format2, buf, size2) - - for i in range(len(self.fields2)): - r[self.fields2[i]] = f2[i] - buf = buf[size2:] - msg._recs.append(r) - if len(buf) != 0: - raise UBloxError("EXTRA_BYTES=%u" % len(buf)) - msg._unpacked = True - - def pack(self, msg, msg_class=None, msg_id=None): - '''pack a UBloxMessage from the .fields and ._recs attributes in msg''' - f1 = [] - if msg_class is None: - msg_class = msg.msg_class() - if msg_id is None: - msg_id = msg.msg_id() - msg._buf = '' - - fields = self.fields[:] - for f in fields: - (fieldname, alen) = ArrayParse(f) - if not fieldname in msg._fields: - break - if alen == -1: - f1.append(msg._fields[fieldname]) - else: - for a in range(alen): - f1.append(msg._fields[fieldname][a]) - try: - # try full length message - fmt = self.msg_format.replace(',', '') - msg._buf = struct.pack(fmt, *tuple(f1)) - except Exception as e: - # try without optional part - fmt = self.msg_format.split(',')[0] - msg._buf = struct.pack(fmt, *tuple(f1)) - - length = len(msg._buf) - if msg._recs: - length += len(msg._recs) * struct.calcsize(self.format2) - header = struct.pack('= level: - print(msg) - - def unpack(self): - '''unpack a message''' - if not self.valid(): - raise UBloxError('INVALID MESSAGE') - type = self.msg_type() - if not type in msg_types: - raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) - msg_types[type].unpack(self) - - def pack(self): - '''pack a message''' - if not self.valid(): - raise UbloxError('INVALID MESSAGE') - type = self.msg_type() - if not type in msg_types: - raise UBloxError('Unknown message %s' % str(type)) - msg_types[type].pack(self) - - def name(self): - '''return the short string name for a message''' - if not self.valid(): - raise UbloxError('INVALID MESSAGE') - type = self.msg_type() - if not type in msg_types: - raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) - return msg_types[type].name - - if PYTHON_VERSION == 2: - def msg_class(self): - '''return the message class''' - return ord(self._buf[2]) - - def msg_id(self): - '''return the message id within the class''' - return ord(self._buf[3]) - else: - def msg_class(self): - '''return the message class''' - return (self._buf[2]) - - def msg_id(self): - '''return the message id within the class''' - return (self._buf[3]) - - def msg_type(self): - '''return the message type tuple (class, id)''' - return (self.msg_class(), self.msg_id()) - - def msg_length(self): - '''return the payload length''' - (payload_length,) = struct.unpack(' 0 and ord(self._buf[0]) != PREAMBLE1: - return False - if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2: - self.debug(1, "bad pre2") - return False - else: - if len(self._buf) > 0 and (self._buf[0]) != PREAMBLE1: - return False - if len(self._buf) > 1 and (self._buf[1]) != PREAMBLE2: - self.debug(1, "bad pre2") - return False - - if self.needed_bytes() == 0 and not self.valid(): - if len(self._buf) > 8: - self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf), self.needed_bytes())) - else: - self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes())) - return False - return True - - def add(self, bytes): - '''add some bytes to a message''' - - self._buf += bytes - while not self.valid_so_far() and len(self._buf) > 0: - '''handle corrupted streams''' - self._buf = self._buf[1:] - if self.needed_bytes() < 0: - self._buf = b"" - - def checksum(self, data=None): - '''return a checksum tuple for a message''' - if data is None: - data = self._buf[2:-2] - cs = 0 - ck_a = 0 - ck_b = 0 - for i in data: - if type(i) is str: - - ck_a = (ck_a + ord(i)) & 0xFF - else: - ck_a = (ck_a + i) & 0xFF - ck_b = (ck_b + ck_a) & 0xFF - return (ck_a, ck_b) - - def valid_checksum(self): - '''check if the checksum is OK''' - (ck_a, ck_b) = self.checksum() - d = self._buf[2:-2] - (ck_a2, ck_b2) = struct.unpack('= 8 and self.needed_bytes() == 0 and self.valid_checksum() - - -class UBlox: - '''main UBlox control class. - - port can be a file (for reading only) or a serial device - ''' - def __init__(self, port, baudrate=115200, timeout=0): - - self.serial_device = port - self.baudrate = baudrate - self.use_sendrecv = False - self.read_only = False - self.use_xfer = False - self.debug_level = 0 - - if self.serial_device.startswith("tcp:"): - import socket - a = self.serial_device.split(':') - destination_addr = (a[1], int(a[2])) - self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.dev.connect(destination_addr) - self.dev.setblocking(1) - self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) - self.use_sendrecv = True - elif os.path.isfile(self.serial_device): - self.read_only = True - self.dev = open(self.serial_device, mode='rb') - if self.serial_device.startswith("spi:"): - import spidev - bus, cs = map(int, self.serial_device.split(':')[1].split('.')) - #print(bus, cs) - self.use_xfer = True - self.dev = spidev.SpiDev() - self.dev.open(bus, cs) - #We reuse baudrate parameter but it's difficult to get default paramaters right. So it's better to specify them explicitly - self.dev.max_speed_hz = baudrate - else: - import serial - self.dev = serial.Serial(self.serial_device, baudrate=self.baudrate, - dsrdtr=False, rtscts=False, xonxoff=False, timeout=timeout) - self.logfile = None - self.log = None - self.preferred_dynamic_model = None - self.preferred_usePPP = None - self.preferred_dgps_timeout = None - - def close(self): - '''close the device''' - self.dev.close() - self.dev = None - - def set_debug(self, debug_level): - '''set debug level''' - self.debug_level = debug_level - - def debug(self, level, msg): - '''write a debug message''' - if self.debug_level >= level: - print(msg) - - def set_logfile(self, logfile, append=False): - '''setup logging to a file''' - if self.log is not None: - self.log.close() - self.log = None - self.logfile = logfile - if self.logfile is not None: - if append: - mode = 'ab' - else: - mode = 'wb' - self.log = open(self.logfile, mode=mode) - - def set_preferred_dynamic_model(self, model): - '''set the preferred dynamic model for receiver''' - self.preferred_dynamic_model = model - if model is not None: - self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) - - def set_preferred_dgps_timeout(self, timeout): - '''set the preferred DGPS timeout for receiver''' - self.preferred_dgps_timeout = timeout - if timeout is not None: - self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) - - def set_preferred_usePPP(self, usePPP): - '''set the preferred usePPP setting for the receiver''' - if usePPP is None: - self.preferred_usePPP = None - return - self.preferred_usePPP = int(usePPP) - self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) - - def nmea_checksum(self, msg): - d = msg[1:] - cs = 0 - for i in d: - cs ^= ord(i) - return cs - - def write(self, buf): - '''write some bytes''' - if not self.read_only: - if self.use_sendrecv: - return self.dev.send(buf) - elif self.use_xfer: - spiBuf = [] # form buf - for b in buf: - if type(b) is str: - - spiBuf.append(ord(b)) - else: - spiBuf.append(b) - return self.dev.xfer2(spiBuf) - return self.dev.write(buf) - - def read(self, n): - '''read some bytes''' - - if self.use_sendrecv: - import socket - try: - buf = self.dev.recv(n) - return buf - except socket.error as e: - return b'' - if self.use_xfer: - buf = self.dev.readbytes(n) - return buf - - buf = self.dev.read(n) - return buf - - def send_nmea(self, msg): - if not self.read_only: - s = msg + "*%02X" % self.nmea_checksum(msg) - - if PYTHON_VERSION == 2: - b = bytearray() - b.extend(s) - else: - b = bytearray() - b.extend(map(ord, s)) - self.write(b) - - def set_binary(self): - '''put a UBlox into binary mode using a NMEA string''' - if not self.read_only: - print("try set binary at %u" % self.baudrate) - self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate) - self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate) - self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate) - self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate) - self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate) - self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate) - - def seek_percent(self, pct): - '''seek to the given percentage of a file''' - self.dev.seek(0, 2) - filesize = self.dev.tell() - self.dev.seek(pct*0.01*filesize) - - def special_handling(self, msg): - '''handle automatic configuration changes''' - if msg.name() == 'CFG_NAV5': - msg.unpack() - sendit = False - pollit = False - if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model: - msg.dynModel = self.preferred_dynamic_model - sendit = True - pollit = True - if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout: - msg.dgpsTimeOut = self.preferred_dgps_timeout - self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut) - sendit = True - # we don't re-poll for this one, as some receivers refuse to set it - if sendit: - msg.pack() - self.send(msg) - if pollit: - self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) - if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None: - msg.unpack() - if msg.usePPP != self.preferred_usePPP: - msg.usePPP = self.preferred_usePPP - msg.mask = 1<<13 - msg.pack() - self.send(msg) - self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) - - def receive_message_nonblocking(self, seconds=5): - '''nonblocking receive of one ublox message''' - with Timeout(seconds=seconds): - return self.receive_message() - - def receive_message(self, ignore_eof=False): - '''blocking receive of one ublox message''' - msg = UBloxMessage() - while True: - n = msg.needed_bytes() - b = self.read(n) - if not b: - if ignore_eof: - time.sleep(0.01) - continue - return None - if self.use_xfer: - if PYTHON_VERSION == 3: - bb = bytearray() - for c in b: - bb.append(c) - b = bb - else: - b = "".join([chr(c) for c in b]) # here str - msg.add(b) - if self.log is not None: - self.log.write(b) - self.log.flush() - if msg.valid(): - self.special_handling(msg) - return msg - - def receive_message_noerror(self, ignore_eof=False): - '''blocking receive of one ublox message, ignoring errors''' - try: - return self.receive_message(ignore_eof=ignore_eof) - except UBloxError as e: - print(e) - return None - except OSError as e: - # Occasionally we get hit with 'resource temporarily unavailable' - # messages here on the serial device, catch them too. - print(e) - return None - - def send(self, msg): - '''send a preformatted ublox message''' - if not msg.valid(): - self.debug(1, "invalid send") - return - if not self.read_only: - self.write(msg._buf) - - def send_message(self, msg_class, msg_id, payload): - '''send a ublox message with class, id and payload''' - msg = UBloxMessage() - msg._buf = struct.pack('