MPU6050 Orientation Sensor

This is a simple project using the Orientation Sensor MPU6050.

This is a 6-axis device containing 3 accelerometers and 3 gyroscopes.

The driver is written directly in Annex script but it rely on the internal fusion algorithm to compute the 3 orientation angles (pitch, roll and yaw).

The files in attachment contains all the required files for this project.

Even if not drawn on the schematics, the code drives also an OLED display that can simply connected in parallel on the same I2C pins.

As the MPU6050 doesn't contains the magnetometers (it is only 6 axis), there will be a little drift on the yaw axis.

Wiring

The output window

The Code

CODE: mpu6050.bas

MPU6050_REG_WHO_AM_I = &h75' parameters for the MPU6050 dim IMU_data(20)AccelX = 0 : AccelY = 0 : AccelZ = 0GyroX = 0: GyroY = 0: GyroZ = 0Temperature = 0' parameters for 6 DoF sensor fusion calculationspitch = 0 : yaw = 0 : roll = 0GyroMeasError = PI * (5.0 / 180.0) ' gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3beta = sqr(3.0 / 4.0) * GyroMeasError ' compute betaGyroMeasDrift = PI * (2.0 / 180.0) ' gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)zeta = sqr(3.0 / 4.0) * GyroMeasDrift ' compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero valuefusion.initfusion.beta = betafusion.zeta = zetaonUrlMessage message'opens the html pagejscall "window.location = 'mpu6050_demo.html'"i2c.setup 4,5oled.init 1oled.print 0,0, "DEMO MPU6050 SENSOR"'check the presence of the MPU6050ident = i2c.ReadRegByte(MPU6050SlaveAddress, MPU6050_REG_WHO_AM_I)if ident <> MPU6050SlaveAddress then print "MPU6050 not found." : end'initialise the moduleMPU6050_Ini for j = 1 to 100000 i2c.readregarray MPU6050SlaveAddress, MPU6050_REG_ACCEL_XOUT_H, 14, IMU_data() ' read all registers in one shot 'extract all the parameters from the buffer extract_param AccelX, 0 extract_param AccelY, 2 extract_param AccelZ, 4 extract_param GyroX, 8 extract_param GyroY, 10 extract_param GyroZ, 12 extract_param Temperature, 6 'scale all the parameters in Engineering Unit AccelX = AccelX/AccelScaleFactor AccelY = AccelY/AccelScaleFactor AccelZ = AccelZ/AccelScaleFactor GyroX = GyroX/GyroScaleFactor GyroY = GyroY/GyroScaleFactor GyroZ = GyroZ/GyroScaleFactor Temperature = Temperature/340 + 36.53 fusion.Madgwick AccelX, AccelY, AccelZ, (GyroX * Pi/180), (GyroY * Pi/180), -(GyroZ * Pi/180) pitch = fusion.angle(1) roll = -fusion.angle(2) yaw = fusion.angle(3) print print str$(pitch, "%3.1f"), str$(roll, "%3.1f"), str$(yaw, "%3.1f") oled.print 0, 20, "Pitch:"+ str$(pitch,"%4.2f")+ " ", 0 oled.print 0, 35, "Roll:"+ str$(roll,"%4.2f")+ " ", 0 oled.print 0, 50, "Yaw:"+ str$(yaw,"%4.2f")+ " ", 0 next jendmessage:UrlMsgReturn str$(pitch) + "," + str$(roll) + "," + str$(yaw) + ",1" return'------------------------------------------------------------'extract a parameter from the buffer at the given address'correct the sign of the value using the Most significant bitsub extract_param(ret, address) ret = (IMU_data(address) <<8) + IMU_data(address+1) if ret > 32768 then ret = ret - 65536end sub'configuration of the MPU6050 sub MPU6050_Ini pause 150 i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_1, &h01 'PLL with X axis gyroscope reference i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_2, &h00 ' no sensors standby i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SMPLRT_DIV, 7 ' sample rate = 8Khz / (1 + 7) -> 1KHz i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_CONFIG, &h01 ' bandwith 5Hz (delay 19msec) Fs 1Kz i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_GYRO_CONFIG, &h08 'set +/-500 degree/second full scale i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_ACCEL_CONFIG, &h00 ' set +/- 2g full scale i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_FIFO_EN, &h00 ' fifo disabled 'enables the Data Ready interrupt, which occurs each 'time a write operation to all of the sensor registers has been completed. 'put a led on the pin INT to see the sample rate i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_INT_ENABLE, &h01 i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SIGNAL_PATH_RESET, &h07 ' reset sensors i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_USER_CTRL, &h00 ' do not reset sensor registers (put 1 if required) 'i2c.WriteRegByte MPU6050SlaveAddress, MPU9250_INT_PIN_CFG, &h02 ' enable bypass mode end sub