--------------------------------------------------------- ---MPU6050&NodeMCU&ESP8266&姿态融合算法整合脚本--- ---读取MPU6050原始数据(三轴加速度，三轴角速度，环境温度) ---融合四元数算法，该程序的输出值为六轴数据融合后的四元数 ---四元数转欧拉角需要在其他外接程序上进行实现 --------------------------------------------------------- id = 0 -- always 0 scl = 6 -- set pin 6 as scl sda = 7 -- set pin 7 as sda MPU6050SlaveAddress = 0x68 AK8963SlaveAddress = 0x0C AccelScaleFactor = 16384; -- sensitivity scale factor respective to full scale setting provided in datasheet GyroScaleFactor = 131; MPU6050_REGISTER_SMPLRT_DIV = 0x19 MPU6050_REGISTER_USER_CTRL = 0x6A MPU6050_REGISTER_PWR_MGMT_1 = 0x6B MPU6050_REGISTER_PWR_MGMT_2 = 0x6C MPU6050_REGISTER_CONFIG = 0x1A MPU6050_REGISTER_GYRO_CONFIG = 0x1B MPU6050_REGISTER_ACCEL_CONFIG = 0x1C AK_8963_CONFIG = 0x18 MPU6050_REGISTER_FIFO_EN = 0x23 MPU6050_REGISTER_INT_ENABLE = 0x38 MPU6050_REGISTER_ACCEL_XOUT_H = 0x3B MPU6050_REGISTER_SIGNAL_PATH_RESET = 0x68 function I2C_Write(deviceAddress, regAddress, data) i2c.start(id) -- send start condition if (i2c.address(id, deviceAddress, i2c.TRANSMITTER))-- set slave address and transmit direction then i2c.write(id, regAddress) -- write address to slave i2c.write(id, data) -- write data to slave i2c.stop(id) -- send stop condition else print("I2C_Write fails") end end function I2C_Read(deviceAddress, regAddress, SizeOfDataToRead) response = 0; i2c.start(id) -- send start condition if (i2c.address(id, deviceAddress, i2c.TRANSMITTER))-- set slave address and transmit direction then i2c.write(id, regAddress) -- write address to slave i2c.stop(id) -- send stop condition i2c.start(id) -- send start condition i2c.address(id, deviceAddress, i2c.RECEIVER)-- set slave address and receive direction response = i2c.read(id, SizeOfDataToRead) -- read defined length response from slave i2c.stop(id) -- send stop condition return response else print("I2C_Read fails") end return response end function unsignTosigned16bit(num) -- convert unsigned 16-bit no. to signed 16-bit no. if num > 32768 then num = num - 65536 end return num end function MPU6050_Init() --configure MPU6050 tmr.delay(150000) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SMPLRT_DIV, 0x07) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_1, 0x01) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_2, 0x00) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_CONFIG, 0x00) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_GYRO_CONFIG, 0x00)-- set +/-250 degree/second full scale I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_CONFIG, 0x00)-- set +/- 2g full scale I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_FIFO_EN, 0x00) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_INT_ENABLE, 0x01) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SIGNAL_PATH_RESET, 0x00) I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_USER_CTRL, 0x00) end i2c.setup(id, sda, scl, i2c.SLOW) -- initialize i2c MPU6050_Init() data = I2C_Read(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_XOUT_H, 14) AccelX = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 1), 8), string.byte(data, 2)))) AccelY = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 3), 8), string.byte(data, 4)))) AccelZ = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 5), 8), string.byte(data, 6)))) Temperature = unsignTosigned16bit(bit.bor(bit.lshift(string.byte(data,7), 8), string.byte(data,8))) GyroX = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 9), 8), string.byte(data, 10)))) GyroY = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 11), 8), string.byte(data, 12)))) GyroZ = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 13), 8), string.byte(data, 14)))) AccelX = AccelX/AccelScaleFactor -- divide each with their sensitivity scale factor AccelY = AccelY/AccelScaleFactor AccelZ = AccelZ/AccelScaleFactor Temperature = Temperature/340+36.53-- temperature formula --角速度的值会产生偏移，需要校准 GyroX = GyroX/GyroScaleFactor+3 GyroY = GyroY/GyroScaleFactor-0.45 GyroZ = GyroZ/GyroScaleFactor+0.24 --姿态融合算法部分定义 Kp = 100.0 Ki = 0.002 halfT = 0.001 q0 = 1 q1 = 0 q2 = 0 q3 = 0 exInt = 0 eyInt = 0 ezInt = 0 roll = 0.0 pitch = 0.0 yaw = 0.0 function updateHardWareDatas() data = I2C_Read(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_XOUT_H, 14) AccelX = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 1), 8), string.byte(data, 2)))) AccelY = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 3), 8), string.byte(data, 4)))) AccelZ = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 5), 8), string.byte(data, 6)))) Temperature = unsignTosigned16bit(bit.bor(bit.lshift(string.byte(data,7), 8), string.byte(data,8))) GyroX = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 9), 8), string.byte(data, 10)))) GyroY = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 11), 8), string.byte(data, 12)))) GyroZ = unsignTosigned16bit((bit.bor(bit.lshift(string.byte(data, 13), 8), string.byte(data, 14)))) AccelX = AccelX/AccelScaleFactor -- divide each with their sensitivity scale factor AccelY = AccelY/AccelScaleFactor AccelZ = AccelZ/AccelScaleFactor Temperature = Temperature/340+36.53-- temperature formula GyroX = GyroX/GyroScaleFactor+3 GyroY = GyroY/GyroScaleFactor-0.45 GyroZ = GyroZ/GyroScaleFactor+0.24 end function IMUupdate(gx,gy,gz,ax,ay,az) -- body -- 测量正常化 norm = math.sqrt(ax*ax + ay*ay + az*az) ax = ax / norm --单位化 ay = ay / norm az = az / norm -- 估计方向的重力 vx = 2*(q1*q3 - q0*q2) vy = 2*(q0*q1 + q2*q3) vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 -- 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和 ex = (ay*vz - az*vy) ey = (az*vx - ax*vz) ez = (ax*vy - ay*vx) -- 积分误差比例积分增益 exInt = exInt + ex*Ki eyInt = eyInt + ey*Ki ezInt = ezInt + ez*Ki -- 调整后的陀螺仪测量 gx = gx + Kp*ex + exInt gy = gy + Kp*ey + eyInt gz = gz + Kp*ez + ezInt -- 整合四元数率和正常化 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT -- 正常化四元 norm = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3) q0 = q0 / norm q1 = q1 / norm q2 = q2 / norm q3 = q3 / norm --pitch = math.asin(-2 * q1 * q3 + 2 * q0* q2) * 57.3 -- pitch ,转换为度数 --roll = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.3 -- rollv --yaw = math.atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3 end while true do --read and print accelero, gyro and temperature value updateHardWareDatas() IMUupdate(GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ) print(string.format("{'q0':%.3g,'q1':%.3g,'q2':%.3g,'q3':%.3g}",q0,q1,q2,q3)) --输出原始数据 --print(string.format("%.3g,%.3g,%.3g",GyroX,GyroY,GyroZ)) --print(string.format("{'GyroX':%.3g,'GyroY':%.3g,'GyroZ':%.3g,'Temperature':%.3g,'AccelX':%.3g,'AccelY':%.3g,'AccelZ':%.3g}", --GyroX, GyroY, GyroZ, Temperature, AccelX, AccelY, AccelZ)) tmr.delay(100000) -- 100ms timer delay end