You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
183 lines
7.6 KiB
183 lines
7.6 KiB
3 years ago
|
---------------------------------------------------------
|
||
|
|
---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
|