├── examples ├── EKF_quaternion │ ├── README.md │ └── EKF_quaternion.py └── GNSS_INS │ ├── GNSS_INS.py │ ├── defaultalgortihm1.png │ └── README.md ├── Shematic └── Pellicanus_SMD_v0.3.pdf ├── systemdrivers ├── SL_871GNSS.py ├── MMC5983MA.py ├── barometer.py ├── IAM_20680.py └── IMU_GNSS.py ├── README.md └── LICENSE /examples/EKF_quaternion/README.md: -------------------------------------------------------------------------------- 1 | The mathematical model will be shared 2 | -------------------------------------------------------------------------------- /examples/GNSS_INS/GNSS_INS.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/acsgn95/PELLICANUS/HEAD/examples/GNSS_INS/GNSS_INS.py -------------------------------------------------------------------------------- /Shematic/Pellicanus_SMD_v0.3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/acsgn95/PELLICANUS/HEAD/Shematic/Pellicanus_SMD_v0.3.pdf -------------------------------------------------------------------------------- /examples/GNSS_INS/defaultalgortihm1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/acsgn95/PELLICANUS/HEAD/examples/GNSS_INS/defaultalgortihm1.png -------------------------------------------------------------------------------- /examples/EKF_quaternion/EKF_quaternion.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/acsgn95/PELLICANUS/HEAD/examples/EKF_quaternion/EKF_quaternion.py -------------------------------------------------------------------------------- /examples/GNSS_INS/README.md: -------------------------------------------------------------------------------- 1 | # GNSS_INS 2 | The sample code here contains the NCL/EKF algorithm with a discrete architecture. 3 | Although the code here is micropython, it is recommended to convert it to C. 4 | Results will be shared. 5 | In addition, this model will be included in the GUI to be developed. 6 | Also this example will be converted to object oriented programming. 7 | 8 | ![defaultalgortihm](https://user-images.githubusercontent.com/78763530/151667659-a49e746b-ea7a-4d92-9b7d-7d5dfb8bc978.png) 9 | -------------------------------------------------------------------------------- /systemdrivers/SL_871GNSS.py: -------------------------------------------------------------------------------- 1 | from machine import UART,Pin,Timer 2 | import time 3 | 4 | gps_module = UART(0,baudrate = 9600, tx = Pin(28), rx = Pin(29)) 5 | 6 | timer = Timer() 7 | 8 | #Used to Store NMEA Sentences 9 | buff = bytearray(255) 10 | latitude = None 11 | longitude = None 12 | height = None 13 | groundspeed = None 14 | gpsyaw = None 15 | geoidalseperation = None 16 | hemisLon = None 17 | hemisLat = None 18 | 19 | def gps(timer): 20 | 21 | global latitude,longitude,height,groundspeed,gpsyaw,geoidalseperation,hemisLon,hemisLat 22 | 23 | latitude = None 24 | longitude = None 25 | height = None 26 | groundspeed = None 27 | gpsyaw = None 28 | geoidalseperation = None 29 | hemisLon = None 30 | hemisLat = None 31 | 32 | gps_module.readline() 33 | 34 | buff = str(gps_module.readline()) 35 | 36 | data = buff.split(",") 37 | 38 | if data[0] == "b'$GNGGA" and len(data) == 15: 39 | 40 | if len(data[2]) != 0 and len(data[4]) != 0 and len(data[9]) != 0 and len(data[11]) != 0: 41 | latitude = data[2] 42 | latdeg = latitude[0:2] 43 | latdeg1 = latitude[2:] 44 | latdeg1 = float(latdeg1)/60 45 | latitude = int(latdeg) + latdeg1 46 | longitude = data[4] 47 | londeg = longitude[0:3] 48 | londeg1 = longitude[3:] 49 | londeg1 = float(londeg1)/60 50 | longitude = int(londeg) + londeg1 51 | 52 | height = float(data[9]) 53 | #print(latdeg,latdeg1,londeg,londeg1) 54 | hemisLat = data[3] 55 | hemisLon = data[5] 56 | geoidalseperation = float(data[11]) 57 | if data[0] == "b'$GNVTG" and len(data) == 10: 58 | if len(data[7]) != 0 and len(data[1]) != 0: 59 | groundspeed = float(data[7]) 60 | gpsyaw = float(data[1]) 61 | 62 | #print(latitude,hemisLat,longitude,hemisLon,height,geoidalseperation,groundspeed,gpsyaw) 63 | print(data) 64 | 65 | reset = Pin(7,Pin.OUT) 66 | 67 | reset.value(1) 68 | time.sleep(1) 69 | gps_module.write("$PMTK101") 70 | gps_module.write("$PMTK258,1,2,115200,1,1") 71 | 72 | timer.init(freq = 4, mode = Timer.PERIODIC, callback=gps) 73 | 74 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PELLICANUS 2 | https://www.crowdsupply.com/g-fusion/pellicanus 3 | 4 | ![crowd-supply-logo-dark@2x](https://user-images.githubusercontent.com/78763530/150209087-3343a52b-b8f7-4014-8455-4775039dd88b.png) 5 | 6 | ![image](https://user-images.githubusercontent.com/78763530/149827798-d9480e51-b02b-4c99-bfbe-de5ced77979b.png) 7 | 8 | ![20211224_154711](https://user-images.githubusercontent.com/78763530/149828149-ae5b037b-489c-4200-96d9-f780288ae33e.jpg) 9 | 10 | 11 | Pellicanus is an open source INS/GPS integrated navigation system designed to be a handy, accessible development board. It consists of 10 DOF IMUs, a GNSS receiver and a RP2040 processor. There are GPIO pins for sensor data input. By default, it includes the loosely coupled extended kalman filter and odometer. It gives you the system’s 3d vector velocity, 3d vector position, and orientation angles. 12 | 13 | Pellicanus is not only a tactical level INS/GPS, it also comes with completely open source mathematical models, software and schematics. Also, the system sensitivity is at a level to compete with military systems on the market. 14 | 15 | Pellicanus can be configured with only a few changes to fit where you want to install it. For example, it can be used in drones, unmanned land vehicles, agricultural systems, or even robotics. 16 | 17 | ![GFusion](https://user-images.githubusercontent.com/78763530/149828035-9aa356ae-d224-4f8a-8faf-f423bed22c54.png) 18 | 19 | # Navigation Algorithm 20 | PELLICANUS includes loosely coupled EKF/NCF with discrete architecture. The architecture is as follows. 21 | 22 | ![defaultalgortihm](https://user-images.githubusercontent.com/78763530/150411685-8fd9774f-e29a-49a3-9d9a-58852d81ff50.png) 23 | 24 | # GUI 25 | We are working on a GUI where you can pull raw IMU data from PELLICANUS and edit them in various AHRS and Navigation programs. 26 | 27 | ![GUI](https://user-images.githubusercontent.com/78763530/156847477-92f87e7e-d96a-417d-a8b9-31e098f5b1f9.png) 28 | ![GUI2](https://user-images.githubusercontent.com/78763530/156847510-41fed9cb-b0c7-42de-bbca-7b360383bcee.png) 29 | 30 | # C Code 31 | In the future, we will share the C driver of the Default Navigation Algorithm and the C code of the sensor drivers. 32 | 33 | ![PUTTY_result](https://user-images.githubusercontent.com/78763530/150804931-6b9ff653-b2a6-47cf-9c71-cb9c94e06441.png) 34 | 35 | # Understanding the Kalman Filter 36 | 37 | The Kalman filter is scary at first. But once you start, everything gets easier. I'm sharing three links that will make it easier for you to understand. 38 | ![1_5CCvZBfEcZfxNlGX2v20MA](https://user-images.githubusercontent.com/78763530/156850388-846dd014-2a9a-4f47-8cb7-067a44d5c7b5.png) 39 | 40 | Alex Becker explains this subject very simply and in a language that everyone can understand. 41 | 42 | https://www.kalmanfilter.net/default.aspx 43 | 44 | I recommend you reinforce it with Michel Van Biezen. 45 | 46 | https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT 47 | 48 | The codes here will allow you to grasp everything along with other filtering methods 49 | 50 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 51 | 52 | # Understanding Navigation Systems 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /systemdrivers/MMC5983MA.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from machine import Pin,I2C,Timer 3 | import uctypes 4 | import ustruct 5 | import utime 6 | 7 | i2c = I2C(1, sda=Pin(26), scl=Pin(27), freq=100000) 8 | 9 | MMC5983MA_XOUT_0 = 0x00 10 | MMC5983MA_XOUT_1 = 0x01 11 | MMC5983MA_YOUT_0 = 0x02 12 | MMC5983MA_YOUT_1 = 0x03 13 | MMC5983MA_ZOUT_0 = 0x04 14 | MMC5983MA_ZOUT_1 = 0x05 15 | MMC5983MA_XYZOUT_2 = 0x06 16 | MMC5983MA_TOUT = 0x07 17 | MMC5983MA_STATUS = 0x08 18 | MMC5983MA_CONTROL_0 = 0x09 19 | MMC5983MA_CONTROL_1 = 0x0A 20 | MMC5983MA_CONTROL_2 = 0x0B 21 | MMC5983MA_CONTROL_3 = 0x0C 22 | MMC5983MA_PRODUCT_ID = 0x2F 23 | 24 | MMC5983MA_ADDRESS = 0x30 25 | 26 | # Sample rates 27 | MODR_ONESHOT = 0x00 28 | MODR_1Hz = 0x01 29 | MODR_10Hz = 0x02 30 | MODR_20Hz = 0x03 31 | MODR_50Hz = 0x04 32 | MODR_100Hz = 0x05 33 | MODR_200Hz = 0x06 # BW = 0x01 only 34 | MODR_1000Hz = 0x07 # BW = 0x11 only 35 | 36 | #Bandwidths 37 | MBW_100Hz = 0x00 # 8 ms measurement time 38 | MBW_200Hz = 0x01 # 4 ms 39 | MBW_400Hz = 0x02 # 2 ms 40 | MBW_800Hz = 0x03 # 0.5 ms 41 | 42 | 43 | # Set/Reset as a function of measurements 44 | MSET_1 = 0x00 # Set/Reset each data measurement 45 | MSET_25 = 0x01 # each 25 data measurements 46 | MSET_75 = 0x02 47 | MSET_100 = 0x03 48 | MSET_250 = 0x04 49 | MSET_500 = 0x05 50 | MSET_1000 = 0x06 51 | MSET_2000 = 0x07 52 | 53 | def I2C_reg_write(i2c, addr, reg, data): 54 | """ 55 | Write bytes to the specified register. 56 | """ 57 | 58 | # Construct message 59 | msg = bytearray() 60 | msg.append(data) 61 | 62 | # Write out message to register 63 | i2c.writeto_mem(addr, reg, msg) 64 | 65 | 66 | 67 | def I2Creg_read(i2c, addr, reg, nbytes=1): 68 | """ 69 | Read byte(s) from specified register. If nbytes > 1, read from consecutive 70 | registers. 71 | """ 72 | 73 | # Check to make sure caller is asking for 1 or more bytes 74 | if nbytes < 1: 75 | return bytearray() 76 | 77 | # Request data from specified register(s) over I2C 78 | data = i2c.readfrom_mem(addr, reg, nbytes) 79 | 80 | return data 81 | 82 | 83 | timer = Timer() 84 | 85 | def MAGNETO_data(): 86 | 87 | XYZout2 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XYZOUT_2,1) 88 | 89 | Xout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XOUT_0,1) 90 | Xout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XOUT_1,1) 91 | Xout0 = int.from_bytes(Xout0,"little")<<8 92 | Xout1 = int.from_bytes(Xout1,"little") 93 | Xout1 = Xout0 | Xout1 94 | X_Magneto = (Xout1<<2) | (((XYZout2[0] & 0xC0) >> 6 ) & 0x3) 95 | 96 | Yout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_YOUT_0,1) 97 | Yout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_YOUT_1,1) 98 | Yout0 = int.from_bytes(Yout0,"little")<<8 99 | Yout1 = int.from_bytes(Yout1,"little") 100 | Yout1 = Yout0 | Yout1 101 | Y_Magneto = (Yout1<<2) | (((XYZout2[0] & 0x30) >> 4 ) & 0x3) 102 | 103 | Zout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_ZOUT_0,1) 104 | Zout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_ZOUT_1,1) 105 | Zout0 = int.from_bytes(Zout0,"little")<<8 106 | Zout1 = int.from_bytes(Zout1,"little") 107 | Zout1 = Zout0 | Zout1 108 | Z_Magneto = (Zout1<<2) | (((XYZout2[0] & 0x03) >> 2 ) & 0x3) 109 | 110 | 111 | print(((X_Magneto-131072)/16384)*100,((Y_Magneto-131072)/16384)*100,((Z_Magneto-131072)/16384)*100) 112 | 113 | 114 | #reset 115 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_CONTROL_0, 0x10) 116 | 117 | utime.sleep(1) 118 | 119 | #set 120 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_CONTROL_0, 0x08) 121 | 122 | utime.sleep(1) 123 | 124 | #init 125 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_0, 0x20 | 0x04) 126 | 127 | utime.sleep(1) 128 | 129 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_1, MBW_100Hz) 130 | 131 | utime.sleep(1) 132 | 133 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_2, 0x08 | MODR_100Hz) 134 | 135 | utime.sleep(1) 136 | ###### 137 | 138 | 139 | 140 | 141 | while True: 142 | 143 | temp = I2Creg_read(i2c,MMC5983MA_ADDRESS, MMC5983MA_STATUS) 144 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_STATUS,(temp[0] & 0x01)) 145 | 146 | MAGNETO_data() 147 | 148 | utime.sleep(0.01) 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | -------------------------------------------------------------------------------- /systemdrivers/barometer.py: -------------------------------------------------------------------------------- 1 | from micropython import const 2 | import utime 3 | import ustruct 4 | import machine 5 | 6 | # list of commands in hex for MS5637 7 | c_reset = const(0x1E) # reset command 8 | r_c1 = const(0xA2) # read PROM C1 command 9 | r_c2 = const(0xA4) # read PROM C2 command 10 | r_c3 = const(0xA6) # read PROM C3 command 11 | r_c4 = const(0xA8) # read PROM C4 command 12 | r_c5 = const(0xAA) # read PROM C5 command 13 | r_c6 = const(0xAC) # read PROM C6 command 14 | r_adc = const(0x00) # read ADC command 15 | r_d1 = const(0x44) # convert D1 (OSR=1024) 16 | r_d2 = const(0x54) # convert D2 (OSR=1024) 17 | 18 | 19 | # set i2c clock to 100KHz 20 | # TE MS5637 i2c adress = 0x76 21 | i2c = machine.I2C(1, sda=machine.Pin(26), scl=machine.Pin(27), freq=100000) 22 | slave_addr = 0x76 23 | 24 | # reset device to make sure PROM loaded 25 | data = bytearray([c_reset]) 26 | i2c.writeto(slave_addr, data) 27 | 28 | # scan i2c bus for active addresses 29 | def scan_I2C(): 30 | devices = i2c.scan() 31 | return devices 32 | 33 | 34 | def read_c1(): #read PROM value C1 35 | data = bytearray([r_c1]) 36 | i2c.writeto(slave_addr, data) 37 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 38 | value = int.from_bytes(raw_c, "big") # use builtin to convert to integer 39 | return value 40 | 41 | 42 | def read_c2(): #read PROM value C2 43 | data = bytearray([r_c2]) 44 | i2c.writeto(slave_addr, data) 45 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 46 | value = int.from_bytes(raw_c, "big") # use builtin to convert to unsigned integer 47 | return value 48 | 49 | def read_c3(): #read PROM value C3 50 | data = bytearray([r_c3]) 51 | i2c.writeto(slave_addr, data) 52 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 53 | value = int.from_bytes(raw_c, "big") # use builtin to convert to unsigned integer 54 | return value 55 | 56 | def read_c4(): #read PROM value C4 57 | data = bytearray([r_c4]) 58 | i2c.writeto(slave_addr, data) 59 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 60 | value = int.from_bytes(raw_c, "big") # use builtin to convert to unsigned integer 61 | return value 62 | 63 | def read_c5(): #read PROM value C5 64 | data = bytearray([r_c5]) 65 | i2c.writeto(slave_addr, data) 66 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 67 | value = int.from_bytes(raw_c, "big") # use builtin to convert to unsigned integer 68 | return value 69 | 70 | def read_c6(): #read PROM value C6 71 | data = bytearray([r_c6]) 72 | i2c.writeto(slave_addr, data) 73 | raw_c = i2c.readfrom(slave_addr, 2) #raw C is 2 bytes 74 | value = int.from_bytes(raw_c, "big") # use builtin to convert to unsigned integer 75 | return value 76 | 77 | # read PROM calibration data 78 | C1 = read_c1() 79 | C2 = read_c2() 80 | C3 = read_c3() 81 | C4 = read_c4() 82 | C5 = read_c5() 83 | C6 = read_c6() 84 | 85 | """print ('PROM C1 = ', C1) 86 | print ('PROM C2 = ', C2) 87 | print ('PROM C3 = ', C3) 88 | print ('PROM C4 = ', C4) 89 | print ('PROM C5 = ', C5) 90 | print ('PROM C6 = ', C6)""" 91 | 92 | # start D1 conversion - pressure (24 bit unsigned) 93 | def start_d1(): 94 | #print ('start D1 ') 95 | data = bytearray([r_d1]) 96 | i2c.writeto(slave_addr, data) 97 | 98 | # start D2 conversion - temperature (24 bit unsigned) 99 | def start_d2(): 100 | #print ('start D2 ') 101 | data = bytearray([r_d2]) 102 | i2c.writeto(slave_addr, data) 103 | 104 | #read ADC 105 | def read_adc(): #read ADC 24 bits unsigned 106 | data = bytearray([r_adc]) 107 | i2c.writeto(slave_addr, data) 108 | adc = i2c.readfrom(slave_addr, 3) #ADC is 3 bytes 109 | value = int.from_bytes(adc, "big") # use builtin to convert to integer 110 | return value 111 | 112 | #print ('i2c scan addresses found: ',scan_I2C()) 113 | 114 | Temp = None 115 | Pres = None 116 | 117 | def calcAirPressure(): 118 | 119 | global Temp,Pres 120 | 121 | start_d1() # start D1 conversion 122 | utime.sleep(1.0) # short delay during conversion 123 | raw_d1 = read_adc() 124 | start_d2() # start D2 conversion 125 | utime.sleep(1.0) 126 | raw_d2 = read_adc() 127 | dT = raw_d2 - (C5 * 256) # difference between actual and ref temp 128 | Temp = (2000 + (dT * (C6/8388608)))/100 #actual temperature 129 | OFF = (C2*131072) + (C4*dT/64) # offset at actual temperature 130 | SENS = (C1*65536) + (C3*dT/128) # pressure offset at actual temperature 131 | Pres = (raw_d1*SENS/2097152 - OFF)/3276800 # barometric pressure 132 | print(Temp,Pres) 133 | utime.sleep(1.0) -------------------------------------------------------------------------------- /systemdrivers/IAM_20680.py: -------------------------------------------------------------------------------- 1 | import machine 2 | import utime 3 | import ustruct 4 | import sys 5 | import uctypes 6 | 7 | 8 | ##### REGISTER MAP ##### 9 | ACCEL_CONFIG = 0x1C 10 | ACCEL_CONFIG2 = 0x1D 11 | GYRO_CONFIG = 0x1B 12 | GYRO_CONFIG2 = 0x1A 13 | 14 | SELF_TEST_X_ACCEL = 0x0D 15 | SELF_TEST_Y_ACCEL = 0x0E 16 | SELF_TEST_Z_ACCEL = 0x0F 17 | 18 | SELF_TEST_X_GYRO = 0x00 19 | SELF_TEST_Y_GYRO = 0x01 20 | SELF_TEST_Z_GYRO = 0X02 21 | 22 | ACCEL_XOUT_H = 0x3B 23 | ACCEL_XOUT_L = 0x3C 24 | ACCEL_YOUT_H = 0x3D 25 | ACCEL_YOUT_L = 0x3E 26 | ACCEL_ZOUT_H = 0x3F 27 | ACCEL_ZOUT_L = 0x40 28 | 29 | GYRO_XOUT_H = 0x43 30 | GYRO_XOUT_L = 0x44 31 | GYRO_YOUT_H = 0x45 32 | GYRO_YOUT_L = 0x46 33 | GYRO_ZOUT_H = 0x47 34 | GYRO_ZOUT_L = 0x48 35 | 36 | PWR_MGMT_1 = 0x6B 37 | PWR_MGMT_2 = 0x6C 38 | 39 | ACCEL_WHO_AM_I = 0x75 40 | GYRO_WHO_AM_I = 0x75 41 | 42 | 43 | 44 | IMU_cs = machine.Pin(17, machine.Pin.OUT) 45 | 46 | # Initialize SPI 47 | 48 | spi = machine.SPI(0, 49 | baudrate=1000000, 50 | polarity=1, 51 | phase=1, 52 | bits=8, 53 | firstbit=machine.SPI.MSB, 54 | sck=machine.Pin(18), 55 | mosi=machine.Pin(19), 56 | miso=machine.Pin(20)) 57 | 58 | 59 | def reg_write(spi, cs, reg, data): 60 | """ 61 | Write 1 byte to the specified register. 62 | """ 63 | 64 | # Construct message (set ~W bit low, MB bit low) 65 | msg = bytearray() 66 | msg.append(0x00 | reg) 67 | msg.append(data) 68 | 69 | # Send out SPI message 70 | cs.value(0) 71 | spi.write(msg) 72 | cs.value(1) 73 | 74 | 75 | 76 | def reg_read(spi, cs, reg, nbytes=1): 77 | """ 78 | Read byte(s) from specified register. If nbytes > 1, read from consecutive 79 | registers. 80 | """ 81 | 82 | # Determine if multiple byte (MB) bit should be set 83 | if nbytes < 1: 84 | return bytearray() 85 | elif nbytes == 1: 86 | mb = 0 87 | else: 88 | mb = 1 89 | 90 | # Construct message (set ~W bit high) 91 | msg = bytearray() 92 | msg.append(0x80 | (mb << 6) | reg) 93 | 94 | # Send out SPI message and read 95 | cs.value(0) 96 | spi.write(msg) 97 | data = spi.read(nbytes) 98 | cs.value(1) 99 | 100 | return data 101 | 102 | 103 | # from stackoverflow J.F. Sebastian 104 | def _twos_comp(val, bits=16): 105 | """compute the 2's complement of int val with bits""" 106 | if (val & (1 << (bits - 1))) != 0: # if sign bit is set 107 | val = val - (1 << bits) # compute negative value 108 | return val 109 | 110 | ####ACCEL SET MAX RANGE######################## 111 | 112 | ####ACCEL SET MAX RANGE######################## 113 | 114 | def ACCEL_set_2g(): 115 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 116 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x00) 117 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 118 | 119 | 120 | def ACCEL_set_4g(): 121 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 122 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x08) 123 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 124 | 125 | 126 | def ACCEL_set_8g(): 127 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 128 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x10) 129 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 130 | 131 | 132 | def ACCEL_set_16g(): 133 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 134 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x18) 135 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 136 | 137 | 138 | ######GYRO SET MAX RANGE########### 139 | 140 | def GYRO_set_250dps(): 141 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 142 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x00) 143 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 144 | 145 | def GYRO_set_500dps(): 146 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 147 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x08) 148 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 149 | 150 | def GYRO_set_1000dps(): 151 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 152 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x10) 153 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 154 | 155 | def GYRO_set_2000dps(): 156 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 157 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x18) 158 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 159 | 160 | 161 | 162 | def ACCEL_data(): 163 | 164 | #################### X axis ############################ 165 | dataX = reg_read(spi, IMU_cs, ACCEL_XOUT_L, 1) 166 | dataX = dataX + reg_read(spi, IMU_cs, ACCEL_XOUT_H, 1) 167 | ax = int.from_bytes(dataX,'little') 168 | ax = _twos_comp(ax) 169 | 170 | #################### Y axis ############################ 171 | dataY = reg_read(spi, IMU_cs, ACCEL_YOUT_L, 1) 172 | dataY = dataY + reg_read(spi, IMU_cs, ACCEL_YOUT_H, 1) 173 | ay = int.from_bytes(dataY,'little') 174 | ay = _twos_comp(ay) 175 | 176 | #################### Z axis ############################ 177 | dataZ = reg_read(spi, IMU_cs, ACCEL_ZOUT_L, 1) 178 | dataZ = dataZ + reg_read(spi, IMU_cs, ACCEL_ZOUT_H, 1) 179 | az = int.from_bytes(dataZ,'little') 180 | az = _twos_comp(az) 181 | 182 | return ax,ay,az 183 | 184 | 185 | def GYRO_data(): 186 | 187 | #################### X axis ############################ 188 | dataX = reg_read(spi, IMU_cs, GYRO_XOUT_L, 1) 189 | dataX = dataX + reg_read(spi, IMU_cs, GYRO_XOUT_H, 1) 190 | wx = int.from_bytes(dataX,'little') 191 | wx = _twos_comp(wx) 192 | 193 | #################### Y axis ############################ 194 | dataY = reg_read(spi, IMU_cs, GYRO_YOUT_L, 1) 195 | dataY = dataY + reg_read(spi, IMU_cs, GYRO_YOUT_H, 1) 196 | wy = int.from_bytes(dataY,'little') 197 | wy = _twos_comp(wy) 198 | 199 | #################### Z axis ############################ 200 | dataZ = reg_read(spi, IMU_cs, GYRO_ZOUT_L, 1) 201 | dataZ = dataZ + reg_read(spi, IMU_cs, GYRO_ZOUT_H, 1) 202 | wz = int.from_bytes(dataZ,'little') 203 | wz = _twos_comp(wz) 204 | 205 | return wx,wy,wz 206 | 207 | 208 | timer = machine.Timer() 209 | 210 | def run(timer): 211 | ax,ay,az = ACCEL_data() 212 | 213 | wx,wy,wz = GYRO_data() 214 | 215 | print(ax,ay,az,wx,wy,wz) 216 | 217 | 218 | ACCEL_set_16g() 219 | utime.sleep(1) 220 | GYRO_set_250dps() 221 | utime.sleep(1) 222 | 223 | timer.init(freq=100, mode=machine.Timer.PERIODIC, callback=run) 224 | 225 | IMU_cs.value(1) 226 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /systemdrivers/IMU_GNSS.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from machine import Pin,I2C,Timer,SPI,UART 3 | import uctypes 4 | import ustruct 5 | import utime 6 | 7 | ##### REGISTER MAP ##### 8 | ##### imu #### 9 | 10 | ACCEL_CONFIG = 0x1C 11 | ACCEL_CONFIG2 = 0x1D 12 | GYRO_CONFIG = 0x1B 13 | GYRO_CONFIG2 = 0x1A 14 | 15 | SELF_TEST_X_ACCEL = 0x0D 16 | SELF_TEST_Y_ACCEL = 0x0E 17 | SELF_TEST_Z_ACCEL = 0x0F 18 | 19 | SELF_TEST_X_GYRO = 0x00 20 | SELF_TEST_Y_GYRO = 0x01 21 | SELF_TEST_Z_GYRO = 0X02 22 | 23 | ACCEL_XOUT_H = 0x3B 24 | ACCEL_XOUT_L = 0x3C 25 | ACCEL_YOUT_H = 0x3D 26 | ACCEL_YOUT_L = 0x3E 27 | ACCEL_ZOUT_H = 0x3F 28 | ACCEL_ZOUT_L = 0x40 29 | 30 | GYRO_XOUT_H = 0x43 31 | GYRO_XOUT_L = 0x44 32 | GYRO_YOUT_H = 0x45 33 | GYRO_YOUT_L = 0x46 34 | GYRO_ZOUT_H = 0x47 35 | GYRO_ZOUT_L = 0x48 36 | 37 | PWR_MGMT_1 = 0x6B 38 | PWR_MGMT_2 = 0x6C 39 | 40 | ACCEL_WHO_AM_I = 0x75 41 | GYRO_WHO_AM_I = 0x75 42 | 43 | #### magnetometer #### 44 | 45 | MMC5983MA_XOUT_0 = 0x00 46 | MMC5983MA_XOUT_1 = 0x01 47 | MMC5983MA_YOUT_0 = 0x02 48 | MMC5983MA_YOUT_1 = 0x03 49 | MMC5983MA_ZOUT_0 = 0x04 50 | MMC5983MA_ZOUT_1 = 0x05 51 | MMC5983MA_XYZOUT_2 = 0x06 52 | MMC5983MA_TOUT = 0x07 53 | MMC5983MA_STATUS = 0x08 54 | MMC5983MA_CONTROL_0 = 0x09 55 | MMC5983MA_CONTROL_1 = 0x0A 56 | MMC5983MA_CONTROL_2 = 0x0B 57 | MMC5983MA_CONTROL_3 = 0x0C 58 | MMC5983MA_PRODUCT_ID = 0x2F 59 | 60 | MMC5983MA_ADDRESS = 0x30 61 | 62 | # Sample rates 63 | MODR_ONESHOT = 0x00 64 | MODR_1Hz = 0x01 65 | MODR_10Hz = 0x02 66 | MODR_20Hz = 0x03 67 | MODR_50Hz = 0x04 68 | MODR_100Hz = 0x05 69 | MODR_200Hz = 0x06 # BW = 0x01 only 70 | MODR_1000Hz = 0x07 # BW = 0x11 only 71 | 72 | #Bandwidths 73 | MBW_100Hz = 0x00 # 8 ms measurement time 74 | MBW_200Hz = 0x01 # 4 ms 75 | MBW_400Hz = 0x02 # 2 ms 76 | MBW_800Hz = 0x03 # 0.5 ms 77 | 78 | 79 | # Set/Reset as a function of measurements 80 | MSET_1 = 0x00 # Set/Reset each data measurement 81 | MSET_25 = 0x01 # each 25 data measurements 82 | MSET_75 = 0x02 83 | MSET_100 = 0x03 84 | MSET_250 = 0x04 85 | MSET_500 = 0x05 86 | MSET_1000 = 0x06 87 | MSET_2000 = 0x07 88 | 89 | ######################################### 90 | 91 | #### PIN CONFIGURATION #### 92 | 93 | #### imu conf #### 94 | IMU_cs = Pin(17, machine.Pin.OUT) 95 | 96 | # Initialize SPI 97 | 98 | spi = SPI(0, 99 | baudrate=1000000, 100 | polarity=1, 101 | phase=1, 102 | bits=8, 103 | firstbit=SPI.MSB, 104 | sck=Pin(18), 105 | mosi=Pin(19), 106 | miso=Pin(20)) 107 | 108 | ################## 109 | 110 | #### mag conf #### 111 | 112 | i2c = I2C(1, sda=Pin(26), scl=Pin(27), freq=100000) 113 | 114 | #### gnss conf #### 115 | 116 | gps_module = UART(0,baudrate = 9600, tx = Pin(28), rx = Pin(29)) 117 | 118 | ########################################### 119 | 120 | #### SPI FUNCTIONS #### 121 | 122 | def reg_write(spi, cs, reg, data): 123 | """ 124 | Write 1 byte to the specified register. 125 | """ 126 | 127 | # Construct message (set ~W bit low, MB bit low) 128 | msg = bytearray() 129 | msg.append(0x00 | reg) 130 | msg.append(data) 131 | 132 | # Send out SPI message 133 | cs.value(0) 134 | spi.write(msg) 135 | cs.value(1) 136 | 137 | 138 | 139 | def reg_read(spi, cs, reg, nbytes=1): 140 | """ 141 | Read byte(s) from specified register. If nbytes > 1, read from consecutive 142 | registers. 143 | """ 144 | 145 | # Determine if multiple byte (MB) bit should be set 146 | if nbytes < 1: 147 | return bytearray() 148 | elif nbytes == 1: 149 | mb = 0 150 | else: 151 | mb = 1 152 | 153 | # Construct message (set ~W bit high) 154 | msg = bytearray() 155 | msg.append(0x80 | (mb << 6) | reg) 156 | 157 | # Send out SPI message and read 158 | cs.value(0) 159 | spi.write(msg) 160 | data = spi.read(nbytes) 161 | cs.value(1) 162 | 163 | return data 164 | 165 | 166 | # from stackoverflow J.F. Sebastian 167 | def _twos_comp(val, bits=16): 168 | """compute the 2's complement of int val with bits""" 169 | if (val & (1 << (bits - 1))) != 0: # if sign bit is set 170 | val = val - (1 << bits) # compute negative value 171 | return val 172 | 173 | ########################################### 174 | 175 | #### I2C FUNCTIONS #### 176 | 177 | def I2C_reg_write(i2c, addr, reg, data): 178 | """ 179 | Write bytes to the specified register. 180 | """ 181 | 182 | # Construct message 183 | msg = bytearray() 184 | msg.append(data) 185 | 186 | # Write out message to register 187 | i2c.writeto_mem(addr, reg, msg) 188 | 189 | 190 | 191 | def I2Creg_read(i2c, addr, reg, nbytes=1): 192 | """ 193 | Read byte(s) from specified register. If nbytes > 1, read from consecutive 194 | registers. 195 | """ 196 | 197 | # Check to make sure caller is asking for 1 or more bytes 198 | if nbytes < 1: 199 | return bytearray() 200 | 201 | # Request data from specified register(s) over I2C 202 | data = i2c.readfrom_mem(addr, reg, nbytes) 203 | 204 | return data 205 | 206 | ########################################### 207 | 208 | ########### IMU FUNCTIONS ################# 209 | 210 | ####ACCEL SET MAX RANGE######################## 211 | 212 | def ACCEL_set_2g(): 213 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 214 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x00) 215 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 216 | 217 | 218 | def ACCEL_set_4g(): 219 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 220 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x08) 221 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 222 | 223 | 224 | def ACCEL_set_8g(): 225 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 226 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x10) 227 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 228 | 229 | 230 | def ACCEL_set_16g(): 231 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 232 | reg_write(spi,IMU_cs,ACCEL_CONFIG,0x18) 233 | reg = reg_read(spi,IMU_cs,ACCEL_CONFIG,1) 234 | 235 | 236 | ######GYRO SET MAX RANGE########### 237 | 238 | def GYRO_set_250dps(): 239 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 240 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x00) 241 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 242 | 243 | def GYRO_set_500dps(): 244 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 245 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x08) 246 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 247 | 248 | def GYRO_set_1000dps(): 249 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 250 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x10) 251 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 252 | 253 | def GYRO_set_2000dps(): 254 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 255 | reg_write(spi,IMU_cs,GYRO_CONFIG,0x18) 256 | reg = reg_read(spi,IMU_cs,GYRO_CONFIG,1) 257 | 258 | 259 | 260 | def ACCEL_data(): 261 | 262 | #################### X axis ############################ 263 | dataX = reg_read(spi, IMU_cs, ACCEL_XOUT_L, 1) 264 | dataX = dataX + reg_read(spi, IMU_cs, ACCEL_XOUT_H, 1) 265 | ax = int.from_bytes(dataX,'little') 266 | ax = _twos_comp(ax) 267 | 268 | #################### Y axis ############################ 269 | dataY = reg_read(spi, IMU_cs, ACCEL_YOUT_L, 1) 270 | dataY = dataY + reg_read(spi, IMU_cs, ACCEL_YOUT_H, 1) 271 | ay = int.from_bytes(dataY,'little') 272 | ay = _twos_comp(ay) 273 | 274 | #################### Z axis ############################ 275 | dataZ = reg_read(spi, IMU_cs, ACCEL_ZOUT_L, 1) 276 | dataZ = dataZ + reg_read(spi, IMU_cs, ACCEL_ZOUT_H, 1) 277 | az = int.from_bytes(dataZ,'little') 278 | az = _twos_comp(az) 279 | 280 | return ax,ay,az 281 | 282 | 283 | def GYRO_data(): 284 | 285 | #################### X axis ############################ 286 | dataX = reg_read(spi, IMU_cs, GYRO_XOUT_L, 1) 287 | dataX = dataX + reg_read(spi, IMU_cs, GYRO_XOUT_H, 1) 288 | wx = int.from_bytes(dataX,'little') 289 | wx = _twos_comp(wx) 290 | 291 | #################### Y axis ############################ 292 | dataY = reg_read(spi, IMU_cs, GYRO_YOUT_L, 1) 293 | dataY = dataY + reg_read(spi, IMU_cs, GYRO_YOUT_H, 1) 294 | wy = int.from_bytes(dataY,'little') 295 | wy = _twos_comp(wy) 296 | 297 | #################### Z axis ############################ 298 | dataZ = reg_read(spi, IMU_cs, GYRO_ZOUT_L, 1) 299 | dataZ = dataZ + reg_read(spi, IMU_cs, GYRO_ZOUT_H, 1) 300 | wz = int.from_bytes(dataZ,'little') 301 | wz = _twos_comp(wz) 302 | 303 | return wx,wy,wz 304 | 305 | ########################################### 306 | 307 | ########### MAG FUNCTIONS ################# 308 | 309 | def MAGNETO_init(): 310 | 311 | #reset 312 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_CONTROL_0, 0x10) 313 | 314 | utime.sleep(1) 315 | 316 | #set 317 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_CONTROL_0, 0x08) 318 | 319 | utime.sleep(1) 320 | 321 | #init 322 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_0, 0x20 | 0x04) 323 | 324 | utime.sleep(1) 325 | 326 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_1, MBW_100Hz) 327 | 328 | utime.sleep(1) 329 | 330 | I2C_reg_write(i2c,MMC5983MA_ADDRESS, MMC5983MA_CONTROL_2, 0x08 | MODR_100Hz) 331 | 332 | utime.sleep(1) 333 | ###### 334 | 335 | 336 | def MAGNETO_data(): 337 | 338 | XYZout2 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XYZOUT_2,1) 339 | 340 | Xout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XOUT_0,1) 341 | Xout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_XOUT_1,1) 342 | Xout0 = int.from_bytes(Xout0,"little")<<8 343 | Xout1 = int.from_bytes(Xout1,"little") 344 | Xout1 = Xout0 | Xout1 345 | X_Magneto = (Xout1<<2) | (((XYZout2[0] & 0xC0) >> 6 ) & 0x3) 346 | 347 | Yout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_YOUT_0,1) 348 | Yout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_YOUT_1,1) 349 | Yout0 = int.from_bytes(Yout0,"little")<<8 350 | Yout1 = int.from_bytes(Yout1,"little") 351 | Yout1 = Yout0 | Yout1 352 | Y_Magneto = (Yout1<<2) | (((XYZout2[0] & 0x30) >> 4 ) & 0x3) 353 | 354 | Zout0 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_ZOUT_0,1) 355 | Zout1 = I2Creg_read(i2c,MMC5983MA_ADDRESS ,MMC5983MA_ZOUT_1,1) 356 | Zout0 = int.from_bytes(Zout0,"little")<<8 357 | Zout1 = int.from_bytes(Zout1,"little") 358 | Zout1 = Zout0 | Zout1 359 | Z_Magneto = (Zout1<<2) | (((XYZout2[0] & 0x03) >> 2 ) & 0x3) 360 | 361 | 362 | return ((X_Magneto-131072)/16384)*100, ((Y_Magneto-131072)/16384)*100, ((Z_Magneto-131072)/16384)*100 363 | 364 | 365 | def MAGNETO_clear(): 366 | 367 | temp = I2Creg_read(i2c,MMC5983MA_ADDRESS, MMC5983MA_STATUS) 368 | I2C_reg_write(i2c,MMC5983MA_ADDRESS ,MMC5983MA_STATUS,(temp[0] & 0x01)) 369 | 370 | ########################################### 371 | 372 | ########### GNSS FUNCTIONS ################# 373 | 374 | gnssTimer = Timer() 375 | isGnssData = False 376 | latitude = None 377 | longitude = None 378 | height = None 379 | groundspeed = None 380 | gpsyaw = None 381 | geoidalseperation = None 382 | hemisLon = None 383 | hemisLat = None 384 | 385 | #Used to Store NMEA Sentences 386 | buff = bytearray(255) 387 | 388 | 389 | def gps(gnssTimer): 390 | 391 | global gpsdata,isGnssData,latitude,longitude,height,groundspeed,gpsyaw,geoidalseperation,hemisLon,hemisLat 392 | 393 | latitude = None 394 | longitude = None 395 | height = None 396 | groundspeed = None 397 | gpsyaw = None 398 | geoidalseperation = None 399 | hemisLon = None 400 | hemisLat = None 401 | 402 | gps_module.readline() 403 | 404 | buff = str(gps_module.readline()) 405 | 406 | data = buff.split(",") 407 | 408 | if data[0] == "b'$GNGGA" and len(data) == 15: 409 | 410 | if len(data[2]) != 0 and len(data[4]) != 0 and len(data[9]) != 0 and len(data[11]) != 0: 411 | latitude = data[2] 412 | latdeg = latitude[0:2] 413 | latdeg1 = latitude[2:] 414 | latdeg1 = float(latdeg1)/60 415 | latitude = int(latdeg) + latdeg1 416 | longitude = data[4] 417 | londeg = longitude[0:3] 418 | londeg1 = longitude[3:] 419 | londeg1 = float(londeg1)/60 420 | longitude = int(londeg) + londeg1 421 | 422 | height = float(data[9]) 423 | #print(latdeg,latdeg1,londeg,londeg1) 424 | hemisLat = data[3] 425 | hemisLon = data[5] 426 | geoidalseperation = float(data[11]) 427 | if data[0] == "b'$GNVTG" and len(data) == 10: 428 | if len(data[7]) != 0 and len(data[1]) != 0: 429 | groundspeed = float(data[7]) 430 | gpsyaw = float(data[1]) 431 | 432 | 433 | isGnssData = True 434 | 435 | 436 | ########################################### 437 | 438 | ########## LED ############# 439 | 440 | led1 = Pin(1,Pin.OUT) 441 | 442 | ########################################### 443 | 444 | ########## GENERAL TIMER FUNCTION ######### 445 | 446 | timer = Timer() 447 | counter = 0 448 | ledcounter = 0 449 | 450 | def allData(timer): 451 | 452 | global isGnssData,counter,ledcounter,latitude,longitude,height,groundspeed,gpsyaw,geoidalseperation,hemisLon,hemisLat 453 | 454 | ##### LED WARNING #### 455 | if (ledcounter % 25 == 0) and (ledcounter % 50 != 0): 456 | led1.high() 457 | 458 | elif (ledcounter % 50 == 0): 459 | led1.low() 460 | 461 | ##################### 462 | 463 | ax,ay,az = ACCEL_data() 464 | 465 | wx,wy,wz = GYRO_data() 466 | 467 | MAGNETO_clear() 468 | 469 | mx,my,mz = MAGNETO_data() 470 | 471 | #### NED CONFLICT and NORM ##### 472 | ax = -ax/4096 473 | ay = -ay/4096 474 | az = -az/4096 475 | wx = -wx/65.5 476 | wy = wy/65.5 477 | wz = -wz/65.5 478 | my = -my 479 | 480 | print(ax,ay,az,wx,wy,wz,mx,my,mz,latitude,longitude,height,groundspeed,gpsyaw,geoidalseperation,hemisLon,hemisLat) 481 | 482 | if isGnssData == True: 483 | 484 | isGnssData = False 485 | latitude = None 486 | longitude = None 487 | height = None 488 | groundspeed = None 489 | gpsyaw = None 490 | geoidalseperation = None 491 | hemisLon = None 492 | hemisLat = None 493 | 494 | counter += 1 495 | ledcounter += 1 496 | 497 | 498 | ########################################### 499 | 500 | ######### INITIALIZATIONS ######## 501 | 502 | ACCEL_set_8g() 503 | utime.sleep(1) 504 | GYRO_set_500dps() 505 | utime.sleep(1) 506 | 507 | MAGNETO_init() 508 | 509 | reset = Pin(7,Pin.OUT) 510 | 511 | reset.value(1) 512 | utime.sleep(1) 513 | gps_module.write("$PMTK101") 514 | 515 | 516 | ####### TIMER BLOCK ########## 517 | 518 | gnssTimer.init(freq = 4, mode = Timer.PERIODIC, callback= gps) 519 | timer.init(freq = 100, mode = Timer.PERIODIC, callback=allData) 520 | 521 | IMU_cs.value(1) --------------------------------------------------------------------------------