├── README.md ├── arduino_imu_firmware └── arduino_imu_firmware.ino ├── boxctrl_6d0f_imu.py └── rpipico_micropython ├── PiicoDev_MPU6050.py ├── PiicoDev_Unified.py └── main.py /README.md: -------------------------------------------------------------------------------- 1 | # Arduino with IMU 2 | Playing around with a 6DOF IMU (MPU-6050), Arduino, Python and OpenGL 3 | 4 | The Arduino Uno is sending pitch and roll data via bluetooth. A python script is receiving the data and displaying a little cube accordingly. The IMU data consists of gyro and accelerometer data, processed by a complementary filter. 5 | 6 | ## Prerequisite Python Libraries 7 | 1. PyOpenGL 8 | 2. Pygame 9 | 3. pySerial 10 | 11 | ## Deployment 12 | 1. connect Arduion with MPU-6050 (connection diagram can be found at https://bit.ly/2VqX6p5) 13 | 2. Connect the Arduino MPU-6050 bundle to PC 14 | 2. In Arduino IDE, uploading **arduino_imu_firmware.ino** 15 | 3. In any Python IDE, run **boxctrl_6d0f_imu.py** 16 | 17 | ## Reference 18 | 19 | You need: 20 | * MPU6050 6DOF Sensor Module "GY-521" for 3,89EUR (ebay, sent from China) 21 | Nice one. LDO regulator on board, takes 5V. 22 | * Data Sheet / Register Map http://invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf 23 | * ArduinoBoard - Gotta love 'em 24 | 25 | This was one of my first experiments. I only used the accelerometer. The reaction is very jerky and not precise at all. 26 | 27 | [![Raw accelerotmeter data](https://img.youtube.com/vi/hf3emXvTad8/0.jpg)](https://www.youtube.com/watch?v=hf3emXvTad8) 28 | 29 | Additionally using the gyros and mixing accelerometer and gyro data with a complementary filter improved precision, smoothness and response sensitivity a lot. As an option I also added yaw data from the gyro but that data is not filtered hence the error accumulates up pretty quickly. 30 | 31 | 32 | [![Filtered accelerotmeter and gyro data](https://img.youtube.com/vi/yqFfmwVufMo/0.jpg)](https://www.youtube.com/watch?v=yqFfmwVufMo) 33 | 34 | This is about 10 years old stuff - Here's the wayback link to my discontinued wiki: 35 | http://web.archive.org/web/20190626080855/http://mattzz.no-ip.org/wiki/Projects/PlayingWithInertialMeasurementUnits 36 | -------------------------------------------------------------------------------- /arduino_imu_firmware/arduino_imu_firmware.ino: -------------------------------------------------------------------------------- 1 | // MPU-6050 Accelerometer + Gyro 2 | 3 | // Bluetooth module connected to digital pins 2,3 4 | // I2C bus on A4, A5 5 | // Servo on pin 0 6 | 7 | #include 8 | //#include 9 | #include 10 | #include 11 | 12 | #define MPU6050_I2C_ADDRESS 0x68 13 | 14 | #define FREQ 30.0 // sample freq in Hz 15 | 16 | // Bluetooth transmitter, used optionally 17 | // SoftwareSerial BTSerial(2, 3); // RX | TX 18 | 19 | Servo roll_servo; 20 | 21 | // global angle, gyro derived 22 | double gSensitivity = 65.5; // for 500 deg/s, check data sheet 23 | double gx = 0, gy = 0, gz = 0; 24 | double gyrX = 0, gyrY = 0, gyrZ = 0; 25 | int16_t accX = 0, accY = 0, accZ = 0; 26 | 27 | double gyrXoffs = -281.00, gyrYoffs = 18.00, gyrZoffs = -83.00; 28 | 29 | void setup() 30 | { 31 | int error; 32 | uint8_t c; 33 | uint8_t sample_div; 34 | 35 | //BTSerial.begin(38400); 36 | Serial.begin(38400); 37 | 38 | // debug led 39 | pinMode(13, OUTPUT); 40 | 41 | // servo 42 | roll_servo.attach(9, 550, 2550); 43 | 44 | // Initialize the 'Wire' class for the I2C-bus. 45 | Wire.begin(); 46 | 47 | // PWR_MGMT_1: 48 | // wake up 49 | i2c_write_reg (MPU6050_I2C_ADDRESS, 0x6b, 0x00); 50 | 51 | // CONFIG: 52 | // Low pass filter samples, 1khz sample rate 53 | i2c_write_reg (MPU6050_I2C_ADDRESS, 0x1a, 0x01); 54 | 55 | // GYRO_CONFIG: 56 | // 500 deg/s, FS_SEL=1 57 | // This means 65.5 LSBs/deg/s 58 | i2c_write_reg(MPU6050_I2C_ADDRESS, 0x1b, 0x08); 59 | 60 | // CONFIG: 61 | // set sample rate 62 | // sample rate FREQ = Gyro sample rate / (sample_div + 1) 63 | // 1kHz / (div + 1) = FREQ 64 | // reg_value = 1khz/FREQ - 1 65 | sample_div = 1000 / FREQ - 1; 66 | i2c_write_reg (MPU6050_I2C_ADDRESS, 0x19, sample_div); 67 | 68 | 69 | // Serial.write("Calibrating..."); 70 | digitalWrite(13, HIGH); 71 | calibrate(); 72 | digitalWrite(13, LOW); 73 | // Serial.write("done."); 74 | } 75 | 76 | void loop() 77 | { 78 | int error; 79 | double dT; 80 | double ax, ay, az; 81 | unsigned long start_time, end_time; 82 | 83 | start_time = millis(); 84 | 85 | read_sensor_data(); 86 | 87 | // angles based on accelerometer 88 | ay = atan2(accX, sqrt( pow(accY, 2) + pow(accZ, 2))) * 180 / M_PI; 89 | ax = atan2(accY, sqrt( pow(accX, 2) + pow(accZ, 2))) * 180 / M_PI; 90 | 91 | // angles based on gyro (deg/s) 92 | gx = gx + gyrX / FREQ; 93 | gy = gy - gyrY / FREQ; 94 | gz = gz + gyrZ / FREQ; 95 | 96 | // complementary filter 97 | // tau = DT*(A)/(1-A) 98 | // = 0.48sec 99 | gx = gx * 0.96 + ax * 0.04; 100 | gy = gy * 0.96 + ay * 0.04; 101 | 102 | // check if there is some kind of request 103 | // from the other side... 104 | if(Serial.available()) 105 | { 106 | char rx_char; 107 | // dummy read 108 | rx_char = Serial.read(); 109 | // we have to send data, as requested 110 | if (rx_char == '.'){ 111 | digitalWrite(13, HIGH); 112 | Serial.print(gx, 2); 113 | Serial.print(", "); 114 | Serial.print(gy, 2); 115 | Serial.print(", "); 116 | Serial.println(gz, 2); 117 | digitalWrite(13, LOW); 118 | } 119 | // reset z gyro axis 120 | if (rx_char == 'z'){ 121 | gz = 0; 122 | } 123 | } 124 | 125 | roll_servo.write(-gx+90); 126 | 127 | end_time = millis(); 128 | 129 | // remaining time to complete sample time 130 | delay(((1/FREQ) * 1000) - (end_time - start_time)); 131 | //Serial.println(end_time - start_time); 132 | } 133 | 134 | 135 | void calibrate(){ 136 | 137 | int x; 138 | long xSum = 0, ySum = 0, zSum = 0; 139 | uint8_t i2cData[6]; 140 | int num = 500; 141 | uint8_t error; 142 | 143 | for (x = 0; x < num; x++){ 144 | 145 | error = i2c_read(MPU6050_I2C_ADDRESS, 0x43, i2cData, 6); 146 | if(error!=0) 147 | return; 148 | 149 | xSum += ((i2cData[0] << 8) | i2cData[1]); 150 | ySum += ((i2cData[2] << 8) | i2cData[3]); 151 | zSum += ((i2cData[4] << 8) | i2cData[5]); 152 | } 153 | gyrXoffs = xSum / num; 154 | gyrYoffs = ySum / num; 155 | gyrZoffs = zSum / num; 156 | 157 | // Serial.println("Calibration result:"); 158 | // Serial.print(gyrXoffs); 159 | // Serial.print(", "); 160 | // Serial.print(gyrYoffs); 161 | // Serial.print(", "); 162 | // Serial.println(gyrZoffs); 163 | 164 | } 165 | 166 | void read_sensor_data(){ 167 | uint8_t i2cData[14]; 168 | uint8_t error; 169 | // read imu data 170 | error = i2c_read(MPU6050_I2C_ADDRESS, 0x3b, i2cData, 14); 171 | if(error!=0) 172 | return; 173 | 174 | // assemble 16 bit sensor data 175 | accX = ((i2cData[0] << 8) | i2cData[1]); 176 | accY = ((i2cData[2] << 8) | i2cData[3]); 177 | accZ = ((i2cData[4] << 8) | i2cData[5]); 178 | 179 | gyrX = (((i2cData[8] << 8) | i2cData[9]) - gyrXoffs) / gSensitivity; 180 | gyrY = (((i2cData[10] << 8) | i2cData[11]) - gyrYoffs) / gSensitivity; 181 | gyrZ = (((i2cData[12] << 8) | i2cData[13]) - gyrZoffs) / gSensitivity; 182 | 183 | } 184 | 185 | // ---- I2C routines 186 | 187 | int i2c_read(int addr, int start, uint8_t *buffer, int size) 188 | { 189 | int i, n, error; 190 | 191 | Wire.beginTransmission(addr); 192 | n = Wire.write(start); 193 | if (n != 1) 194 | return (-10); 195 | 196 | n = Wire.endTransmission(false); // hold the I2C-bus 197 | if (n != 0) 198 | return (n); 199 | 200 | // Third parameter is true: relase I2C-bus after data is read. 201 | Wire.requestFrom(addr, size, true); 202 | i = 0; 203 | while(Wire.available() && i= 0x8000): 55 | return -((65535 - y) + 1) 56 | else: 57 | return y 58 | 59 | 60 | class PiicoDev_MPU6050(object): 61 | def __init__(self, bus=None, freq=None, sda=None, scl=None, addr=_MPU6050_ADDRESS): 62 | self._failCount = 0 63 | self._terminatingFailCount = 0 64 | try: 65 | if compat_ind >= 1: 66 | pass 67 | else: 68 | print(compat_str) 69 | except: 70 | print(compat_str) 71 | self.i2c = create_unified_i2c(bus=bus, freq=freq, sda=sda, scl=scl) 72 | self.addr = addr 73 | try: 74 | # Wake up the MPU-6050 since it starts in sleep mode 75 | self.i2c.writeto_mem(self.addr, _PWR_MGMT_1, bytes([0x00])) 76 | sleep_ms(5) 77 | except Exception as e: 78 | print(i2c_err_str.format(self.addr)) 79 | raise e 80 | self._accel_range = self.get_accel_range(True) 81 | self._gyro_range = self.get_gyro_range(True) 82 | 83 | def _readData(self, register): 84 | failCount = 0 85 | while failCount < _maxFails: 86 | try: 87 | sleep_ms(10) 88 | data = self.i2c.readfrom_mem(self.addr, register, 6) 89 | break 90 | except: 91 | failCount = failCount + 1 92 | self._failCount = self._failCount + 1 93 | if failCount >= _maxFails: 94 | self._terminatingFailCount = self._terminatingFailCount + 1 95 | print(i2c_err_str.format(self.addr)) 96 | return {'x': float('NaN'), 'y': float('NaN'), 'z': float('NaN')} 97 | x = signedIntFromBytes(data[0:2]) 98 | y = signedIntFromBytes(data[2:4]) 99 | z = signedIntFromBytes(data[4:6]) 100 | return {'x': x, 'y': y, 'z': z} 101 | 102 | # Reads the temperature from the onboard temperature sensor of the MPU-6050. 103 | # Returns the temperature [degC]. 104 | def read_temperature(self): 105 | try: 106 | rawData = self.i2c.readfrom_mem(self.addr, _TEMP_OUT0, 2) 107 | raw_temp = (signedIntFromBytes(rawData, 'big')) 108 | except: 109 | print(i2c_err_str.format(self.addr)) 110 | return float('NaN') 111 | actual_temp = (raw_temp / 340) + 36.53 112 | return actual_temp 113 | 114 | # Sets the range of the accelerometer 115 | # accel_range : the range to set the accelerometer to. Using a pre-defined range is advised. 116 | def set_accel_range(self, accel_range): 117 | self.i2c.writeto_mem(self.addr, _ACCEL_CONFIG, bytes([accel_range])) 118 | self._accel_range = accel_range 119 | 120 | # Gets the range the accelerometer is set to. 121 | # raw=True: Returns raw value from the ACCEL_CONFIG register 122 | # raw=False: Return integer: -1, 2, 4, 8 or 16. When it returns -1 something went wrong. 123 | def get_accel_range(self, raw = False): 124 | # Get the raw value 125 | raw_data = self.i2c.readfrom_mem(self.addr, _ACCEL_CONFIG, 2) 126 | 127 | if raw is True: 128 | return raw_data[0] 129 | elif raw is False: 130 | if raw_data[0] == _ACC_RNG_2G: 131 | return 2 132 | elif raw_data[0] == _ACC_RNG_4G: 133 | return 4 134 | elif raw_data[0] == _ACC_RNG_8G: 135 | return 8 136 | elif raw_data[0] == _ACC_RNG_16G: 137 | return 16 138 | else: 139 | return -1 140 | 141 | # Reads and returns the X, Y and Z values from the accelerometer. 142 | # Returns dictionary data in g or m/s^2 (g=False) 143 | def read_accel_data(self, g = False): 144 | accel_data = self._readData(_ACCEL_XOUT0) 145 | accel_range = self._accel_range 146 | scaler = None 147 | if accel_range == _ACC_RNG_2G: 148 | scaler = _ACC_SCLR_2G 149 | elif accel_range == _ACC_RNG_4G: 150 | scaler = _ACC_SCLR_4G 151 | elif accel_range == _ACC_RNG_8G: 152 | scaler = _ACC_SCLR_8G 153 | elif accel_range == _ACC_RNG_16G: 154 | scaler = _ACC_SCLR_16G 155 | else: 156 | print("Unkown range - scaler set to _ACC_SCLR_2G") 157 | scaler = _ACC_SCLR_2G 158 | 159 | x = accel_data['x'] / scaler 160 | y = accel_data['y'] / scaler 161 | z = accel_data['z'] / scaler 162 | 163 | if g is True: 164 | return {'x': x, 'y': y, 'z': z} 165 | elif g is False: 166 | x = x * _GRAVITIY_MS2 167 | y = y * _GRAVITIY_MS2 168 | z = z * _GRAVITIY_MS2 169 | return {'x': x, 'y': y, 'z': z} 170 | 171 | def read_accel_abs(self, g=False): 172 | d=self.read_accel_data(g) 173 | return sqrt(d['x']**2+d['y']**2+d['z']**2) 174 | 175 | def set_gyro_range(self, gyro_range): 176 | self.i2c.writeto_mem(self.addr, _GYRO_CONFIG, bytes([gyro_range])) 177 | self._gyro_range = gyro_range 178 | 179 | # Gets the range the gyroscope is set to. 180 | # raw=True: return raw value from GYRO_CONFIG register 181 | # raw=False: return range in deg/s 182 | def get_gyro_range(self, raw = False): 183 | # Get the raw value 184 | raw_data = self.i2c.readfrom_mem(self.addr, _GYRO_CONFIG, 2) 185 | 186 | if raw is True: 187 | return raw_data[0] 188 | elif raw is False: 189 | if raw_data[0] == _GYR_RNG_250DEG: 190 | return 250 191 | elif raw_data[0] == _GYR_RNG_500DEG: 192 | return 500 193 | elif raw_data[0] == _GYR_RNG_1000DEG: 194 | return 1000 195 | elif raw_data[0] == _GYR_RNG_2000DEG: 196 | return 2000 197 | else: 198 | return -1 199 | 200 | # Gets and returns the X, Y and Z values from the gyroscope. 201 | # Returns the read values in a dictionary. 202 | def read_gyro_data(self): 203 | gyro_data = self._readData(_GYRO_XOUT0) 204 | gyro_range = self._gyro_range 205 | scaler = None 206 | if gyro_range == _GYR_RNG_250DEG: 207 | scaler = _GYR_SCLR_250DEG 208 | elif gyro_range == _GYR_RNG_500DEG: 209 | scaler = _GYR_SCLR_500DEG 210 | elif gyro_range == _GYR_RNG_1000DEG: 211 | scaler = _GYR_SCLR_1000DEG 212 | elif gyro_range == _GYR_RNG_2000DEG: 213 | scaler = _GYR_SCLR_2000DEG 214 | else: 215 | print("Unkown range - scaler set to _GYR_SCLR_250DEG") 216 | scaler = _GYR_SCLR_250DEG 217 | 218 | x = gyro_data['x'] / scaler 219 | y = gyro_data['y'] / scaler 220 | z = gyro_data['z'] / scaler 221 | 222 | return {'x': x, 'y': y, 'z': z} 223 | 224 | def read_angle(self): # returns radians. orientation matches silkscreen 225 | a=self.read_accel_data() 226 | x=atan2(a['y'],a['z']) 227 | y=atan2(-a['x'],a['z']) 228 | return {'x': x, 'y': y} -------------------------------------------------------------------------------- /rpipico_micropython/PiicoDev_Unified.py: -------------------------------------------------------------------------------- 1 | ''' 2 | PiicoDev.py: Unifies I2C drivers for different builds of MicroPython 3 | Changelog: 4 | - 2021 M.Ruppe - Initial Unified Driver 5 | - 2022-10-13 P.Johnston - Add helptext to run i2csetup script on Raspberry Pi 6 | - 2022-10-14 M.Ruppe - Explicitly set default I2C initialisation parameters for machine-class (Raspberry Pi Pico + W) 7 | - 2023-01-31 L.Howell - Add minimal support for ESP32 8 | - 2023-05-17 M.Ruppe - Make I2CUnifiedMachine() more flexible on initialisation. Frequency is optional. 9 | - 2023-12-20 M.Taylor - added scan() function for quick userland test of connected i2c modules 10 | ''' 11 | import os 12 | _SYSNAME = os.uname().sysname 13 | compat_ind = 1 14 | i2c_err_str = 'PiicoDev could not communicate with module at address 0x{:02X}, check wiring' 15 | setupi2c_str = ', run "sudo curl -L https://piico.dev/i2csetup | bash". Suppress this warning by setting suppress_warnings=True' 16 | 17 | if _SYSNAME == 'microbit': 18 | from microbit import i2c 19 | from utime import sleep_ms 20 | 21 | elif _SYSNAME == 'Linux': 22 | from smbus2 import SMBus, i2c_msg 23 | from time import sleep 24 | from math import ceil 25 | 26 | def sleep_ms(t): 27 | sleep(t/1000) 28 | 29 | else: 30 | from machine import I2C, Pin 31 | from utime import sleep_ms 32 | 33 | class I2CBase: 34 | def writeto_mem(self, addr, memaddr, buf, *, addrsize=8): 35 | raise NotImplementedError('writeto_mem') 36 | 37 | def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8): 38 | raise NotImplementedError('readfrom_mem') 39 | 40 | def write8(self, addr, buf, stop=True): 41 | raise NotImplementedError('write') 42 | 43 | def read16(self, addr, nbytes, stop=True): 44 | raise NotImplementedError('read') 45 | 46 | def __init__(self, bus=None, freq=None, sda=None, scl=None): 47 | raise NotImplementedError('__init__') 48 | 49 | class I2CUnifiedMachine(I2CBase): 50 | def __init__(self, bus=None, freq=None, sda=None, scl=None): 51 | if _SYSNAME == 'esp32' and (bus is None or sda is None or scl is None): 52 | raise Exception('Please input bus, machine.pin SDA, and SCL objects to use ESP32') 53 | 54 | if freq is None: freq = 400_000 55 | if not isinstance(freq, (int)): 56 | raise ValueError("freq must be an Int") 57 | if freq < 400_000: print("\033[91mWarning: minimum freq 400kHz is recommended if using OLED module.\033[0m") 58 | if bus is not None and sda is not None and scl is not None: 59 | print('Using supplied bus, sda, and scl to create machine.I2C() with freq: {} Hz'.format(freq)) 60 | self.i2c = I2C(bus, freq=freq, sda=sda, scl=scl) 61 | elif bus is None and sda is None and scl is None: 62 | self.i2c = I2C(0, scl=Pin(9), sda=Pin(8), freq=freq) # RPi Pico in Expansion Board 63 | else: 64 | raise Exception("Please provide at least bus, sda, and scl") 65 | 66 | self.writeto_mem = self.i2c.writeto_mem 67 | self.readfrom_mem = self.i2c.readfrom_mem 68 | 69 | def write8(self, addr, reg, data): 70 | if reg is None: 71 | self.i2c.writeto(addr, data) 72 | else: 73 | self.i2c.writeto(addr, reg + data) 74 | 75 | def read16(self, addr, reg): 76 | self.i2c.writeto(addr, reg, False) 77 | return self.i2c.readfrom(addr, 2) 78 | 79 | def scan(self): 80 | print([hex(i) for i in self.i2c.scan()]) 81 | 82 | class I2CUnifiedMicroBit(I2CBase): 83 | def __init__(self, freq=None): 84 | if freq is not None: 85 | print('Initialising I2C freq to {}'.format(freq)) 86 | microbit.i2c.init(freq=freq) 87 | 88 | def writeto_mem(self, addr, memaddr, buf, *, addrsize=8): 89 | ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit 90 | i2c.write(addr, ad + buf) 91 | 92 | def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8): 93 | ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit 94 | i2c.write(addr, ad, repeat=True) 95 | return i2c.read(addr, nbytes) 96 | 97 | def write8(self, addr, reg, data): 98 | if reg is None: 99 | i2c.write(addr, data) 100 | else: 101 | i2c.write(addr, reg + data) 102 | 103 | def read16(self, addr, reg): 104 | i2c.write(addr, reg, repeat=True) 105 | return i2c.read(addr, 2) 106 | 107 | def scan(self): 108 | print([hex(i) for i in self.i2c.scan()]) 109 | 110 | class I2CUnifiedLinux(I2CBase): 111 | def __init__(self, bus=None, suppress_warnings=True): 112 | if suppress_warnings == False: 113 | with open('/boot/config.txt') as config_file: 114 | if 'dtparam=i2c_arm=on' in config_file.read(): 115 | pass 116 | else: 117 | print('I2C is not enabled. To enable' + setupi2c_str) 118 | config_file.close() 119 | with open('/boot/config.txt') as config_file: 120 | if 'dtparam=i2c_arm_baudrate=400000' in config_file.read(): 121 | pass 122 | else: 123 | print('Slow baudrate detected. If glitching occurs' + setupi2c_str) 124 | config_file.close() 125 | if bus is None: 126 | bus = 1 127 | self.i2c = SMBus(bus) 128 | 129 | def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8): 130 | data = [None] * nbytes # initialise empty list 131 | self.smbus_i2c_read(addr, memaddr, data, nbytes, addrsize=addrsize) 132 | return data 133 | 134 | def writeto_mem(self, addr, memaddr, buf, *, addrsize=8): 135 | self.smbus_i2c_write(addr, memaddr, buf, len(buf), addrsize=addrsize) 136 | 137 | def smbus_i2c_write(self, address, reg, data_p, length, addrsize=8): 138 | ret_val = 0 139 | data = [] 140 | for index in range(length): 141 | data.append(data_p[index]) 142 | if addrsize == 8: 143 | msg_w = i2c_msg.write(address, [reg] + data) 144 | elif addrsize == 16: 145 | msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff] + data) 146 | else: 147 | raise Exception('address must be 8 or 16 bits long only') 148 | self.i2c.i2c_rdwr(msg_w) 149 | return ret_val 150 | 151 | def smbus_i2c_read(self, address, reg, data_p, length, addrsize=8): 152 | ret_val = 0 153 | if addrsize == 8: 154 | msg_w = i2c_msg.write(address, [reg]) # warning this is set up for 16-bit addresses 155 | elif addrsize == 16: 156 | msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff]) # warning this is set up for 16-bit addresses 157 | else: 158 | raise Exception('address must be 8 or 16 bits long only') 159 | msg_r = i2c_msg.read(address, length) 160 | self.i2c.i2c_rdwr(msg_w, msg_r) 161 | if ret_val == 0: 162 | for index in range(length): 163 | data_p[index] = ord(msg_r.buf[index]) 164 | return ret_val 165 | 166 | def write8(self, addr, reg, data): 167 | if reg is None: 168 | d = int.from_bytes(data, 'big') 169 | self.i2c.write_byte(addr, d) 170 | else: 171 | r = int.from_bytes(reg, 'big') 172 | d = int.from_bytes(data, 'big') 173 | self.i2c.write_byte_data(addr, r, d) 174 | 175 | def read16(self, addr, reg): 176 | regInt = int.from_bytes(reg, 'big') 177 | return self.i2c.read_word_data(addr, regInt).to_bytes(2, byteorder='little', signed=False) 178 | 179 | def scan(self): 180 | print([hex(i) for i in self.i2c.scan()]) 181 | 182 | 183 | def create_unified_i2c(bus=None, freq=None, sda=None, scl=None, suppress_warnings=True): 184 | if _SYSNAME == 'microbit': 185 | i2c = I2CUnifiedMicroBit(freq=freq) 186 | elif _SYSNAME == 'Linux': 187 | i2c = I2CUnifiedLinux(bus=bus, suppress_warnings=suppress_warnings) 188 | else: 189 | i2c = I2CUnifiedMachine(bus=bus, freq=freq, sda=sda, scl=scl) 190 | return i2c -------------------------------------------------------------------------------- /rpipico_micropython/main.py: -------------------------------------------------------------------------------- 1 | import math 2 | from PiicoDev_MPU6050 import PiicoDev_MPU6050 3 | import time 4 | import sys 5 | import uselect 6 | 7 | # Initialize MPU6050 (bus=1, sda=14, scl=15) 8 | mpu = PiicoDev_MPU6050(bus=1, sda=14, scl=15) 9 | 10 | # Calibrate gyro to calculate offsets in deg/s 11 | def calibrate_gyro(num_samples=500): 12 | gx_total = gy_total = gz_total = 0.0 13 | for _ in range(num_samples): 14 | gyro = mpu.read_gyro_data() 15 | gx_total += gyro['x'] 16 | gy_total += gyro['y'] 17 | gz_total += gyro['z'] 18 | time.sleep_ms(1) 19 | return (gx_total/num_samples, gy_total/num_samples, gz_total/num_samples) 20 | 21 | gyrX_offset, gyrY_offset, gyrZ_offset = calibrate_gyro() 22 | 23 | # Complementary filter variables 24 | gx = gy = gz = 0.0 25 | FREQ = 30.0 26 | dt = 1.0 / FREQ 27 | 28 | # Setup non-blocking input for serial requests 29 | poll = uselect.poll() 30 | poll.register(sys.stdin, uselect.POLLIN) 31 | 32 | while True: 33 | start_time = time.ticks_ms() 34 | 35 | # Read accelerometer and calculate angles 36 | accel = mpu.read_accel_data(g=True) 37 | ax, ay, az = accel['x'], accel['y'], accel['z'] 38 | 39 | roll_acc = math.degrees(math.atan2(ay, math.sqrt(ax**2 + az**2))) 40 | pitch_acc = math.degrees(math.atan2(ax, math.sqrt(ay**2 + az**2))) 41 | 42 | # Read and calibrate gyro data (deg/s) 43 | gyro = mpu.read_gyro_data() 44 | gyrX = gyro['x'] - gyrX_offset 45 | gyrY = -(gyro['y'] - gyrY_offset) # Negative sign to match Arduino orientation 46 | gyrZ = gyro['z'] - gyrZ_offset 47 | 48 | # Integrate gyro data 49 | gx += gyrX * dt 50 | gy += gyrY * dt 51 | gz += gyrZ * dt 52 | 53 | # Apply complementary filter (96% gyro, 4% accelerometer) 54 | alpha = 0.96 55 | gx = gx * alpha + roll_acc * (1 - alpha) 56 | gy = gy * alpha + pitch_acc * (1 - alpha) 57 | 58 | # Check for incoming serial commands 59 | if poll.poll(0): 60 | cmd = sys.stdin.read(1) 61 | if cmd == '.': 62 | print("{:.2f}, {:.2f}, {:.2f}".format(gx, gy, gz)) 63 | elif cmd == 'z': 64 | gz = 0 # Reset yaw 65 | 66 | # Maintain sample rate 67 | elapsed = time.ticks_diff(time.ticks_ms(), start_time) 68 | remaining = int(1000/FREQ) - elapsed 69 | if remaining > 0: 70 | time.sleep_ms(remaining) --------------------------------------------------------------------------------