# coding=gbk
import smbus2
import serial
import time
import math

ser = serial.Serial('/dev/ttyAMA0', 115200)  # A200 DK A2串口初始化

# 初始化加速度计和陀螺仪的权重
k = 0.3

# 初始化欧拉角
roll_acc = 0.0
pitch_acc = 0.0
roll_gyro = 0.0
pitch_gyro = 0.0
yaw_gyro = 0.0
gyro_x_last = 0.0
gyro_y_last = 0.0
gyro_z_last = 0.0

# 初始化时间戳
timestamp = time.time()

# MPU6050 地址
MPU6050_ADDR = 0x68
# 寄存器地址
REG_PWR_MGMT_1 = 0x6B
REG_ACCEL_XOUT_H = 0x3B
REG_ACCEL_YOUT_H = 0x3D
REG_ACCEL_ZOUT_H = 0x3F
REG_TEMP_OUT_H = 0x41
REG_GYRO_XOUT_H = 0x43
REG_GYRO_YOUT_H = 0x45
REG_GYRO_ZOUT_H = 0x47
# 灵敏度参数
ACCEL_SENSITIVITY = 16384.0
GYRO_SENSITIVITY = 131.0
TEMP_SENSITIVITY = 36.53
# 初始化I2C总线
bus = smbus2.SMBus(7)
# 唤醒传感器
bus.write_byte_data(MPU6050_ADDR, REG_PWR_MGMT_1, 0)


# 从MPU6050寄存器中读取一个16位有符号整数
def read_word(reg):
    high = bus.read_byte_data(MPU6050_ADDR, reg)
    low = bus.read_byte_data(MPU6050_ADDR, reg + 1)
    value = (high << 8) + low
    # 若为负数，补码转换：取反加1
    if value >= 0x8000:
        return -((65535 - value) + 1)
    else:
        return value


def read_data():
    # 从传感器读取六轴数据
    accel_x = read_word(REG_ACCEL_XOUT_H)
    accel_y = read_word(REG_ACCEL_YOUT_H)
    accel_z = read_word(REG_ACCEL_ZOUT_H)
    temp = read_word(REG_TEMP_OUT_H)
    gyro_x = read_word(REG_GYRO_XOUT_H)
    gyro_y = read_word(REG_GYRO_YOUT_H)
    gyro_z = read_word(REG_GYRO_ZOUT_H)
    # 单位转换
    accel_x = accel_x / ACCEL_SENSITIVITY
    accel_y = accel_y / ACCEL_SENSITIVITY
    accel_z = accel_z / ACCEL_SENSITIVITY
    temp = (temp / 340.0) + TEMP_SENSITIVITY
    gyro_x = gyro_x / GYRO_SENSITIVITY
    gyro_y = gyro_y / GYRO_SENSITIVITY
    gyro_z = gyro_z / GYRO_SENSITIVITY
    return accel_x, accel_y, accel_z, temp, gyro_x, gyro_y, gyro_z


def get_roll_pitch_yaw(accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z):

    global timestamp, gyro_x_last, gyro_y_last, gyro_z_last, roll_gyro, pitch_gyro, yaw_gyro

    # 计算加速度计的欧拉角
    roll_acc = math.atan2(accel_y, accel_z) * 180 / math.pi
    pitch_acc = math.atan2(-accel_x, math.sqrt(accel_y ** 2 + accel_z ** 2)) * 180 / math.pi

    # 计算时间间隔
    dt = time.time() - timestamp
    timestamp = time.time()

    # 计算陀螺仪的欧拉角变化量
    roll_gyro += (gyro_x + gyro_x_last) * dt / 2
    pitch_gyro += (gyro_y + gyro_y_last) * dt / 2
    yaw_gyro += (gyro_z + gyro_z_last) * dt / 2

    gyro_x_last = gyro_x
    gyro_y_last = gyro_y
    gyro_z_last = gyro_z

    # 使用加速度计和陀螺仪的数据进行姿态融合
    roll = k * roll_gyro + (1 - k) * roll_acc
    pitch = k * pitch_gyro + (1 - k) * pitch_acc
    yaw = yaw_gyro

    return roll, pitch, yaw


while True:
    accel_x, accel_y, accel_z, temp, gyro_x, gyro_y, gyro_z = read_data()
    roll, pitch, yaw = get_roll_pitch_yaw(accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z)
    timestamp = time.time()
    # 串口输出
    # 将姿态数据转换为字符串并发送
    data = str(roll) + ',' + str(pitch) + ',' + str(yaw) + '\n'
    ser.write(data.encode())
    # Print data
    print("Accelerometer (g): X=%.2f Y=%.2f Z=%.2f" % (accel_x, accel_y, accel_z))
    print("Temperature (C): %.2f" % temp)
    print("Gyroscope (deg/s): X=%.2f Y=%.2f Z=%.2f" % (gyro_x, gyro_y, gyro_z))
    print("roll:", roll, "pitch:", pitch, "yaw:", yaw)
    # 设置检测时间间隔
    time.sleep(0.05)