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.
182 lines
7.6 KiB
182 lines
7.6 KiB
--------------------------------------------------------- |
|
---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
|
|
|