#include "SC7A20.h"
#include "iic.h"
#include "stdio.h"
#include <math.h>

void SC7A20_Init(void) {	
	uint8_t ctrl1 = 0x2F; // 1-1HZ,2-10HZ,5-100HZ,6-200HZ，//F为开低功耗//0x27
	uint8_t ctrl4 = 0x20; // ±8g full scale//10为4g,20为8g，30为16g。//0x10
	uint8_t ctrl2 = 0x00; // 0x00关闭高通滤波//0x09为开启高通滤波

    // 配置CTRL_REG1寄存器0X20
    Soft_I2C_Write(SC7A20_ADDR, SC7A20_CTRL_REG1, ctrl1);

    // 配置CTRL_REG4寄存器0X23
    Soft_I2C_Write(SC7A20_ADDR, SC7A20_CTRL_REG4, ctrl4);

    // 配置CTRL_REG2寄存器0X21
    Soft_I2C_Write(SC7A20_ADDR, SC7A20_CTRL_REG2, ctrl2);

}
void SC7A20_ReadXYZ(int16_t *x, int16_t *y, int16_t *z) {
    uint8_t data[6];
    Soft_I2C_ReadMulti(SC7A20_ADDR, SC7A20_OUT_X_L | 0x80 , data, 6);
    *x = SC7A20_Class_12bitComplement(data[1], data[0]);
    *y = SC7A20_Class_12bitComplement(data[3], data[2]);
    *z = SC7A20_Class_12bitComplement(data[5], data[4]);
		*x = *x + 1500;
		*z = *z + 3000;
}
int16_t SC7A20_Class_12bitComplement(uint8_t msb, uint8_t lsb) {
    int16_t temp = (msb << 8) | lsb;
    temp >>= 4; // 将数据右移 4 位，保留高 12 位有效数据
    if (temp & 0x0800) { // 检查最高位（第 12 位），如果是 1，表示负数
        temp |= 0xF000; // 将高 4 位补为 1，得到正确的负数
    }
    return temp;
}