传感器驱动 MPU9250：SPI 通信、寄存器配置与标度转换¶

                ┌───────────── MPU9250 封装 ─────────────┐
                │                                        │
  STM32 SPI1 ──┼──► [MPU6500 六轴]                        │
  (SCK/MOSI/   │     · 3轴加速度计 (±2/±4/±8/±16 g)      │
   MISO/CS)    │     · 3轴陀螺仪 (±250/±500/±1000/±2000°/s)│
                │     · 温度传感器                         │
                │     · 内部 I2C 主控制器 ──┐              │
                │                          │              │
                │     [AK8963 三轴磁力计] ◄─┘              │
                │     · 14-bit / 16-bit 输出              │
                │     · 量程 ±4800 μT                    │
                │     · 内置灵敏度修正 (ASA)              │
                └────────────────────────────────────────┘

// mcu_init.c L211-219
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;  // 全双工
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;                        // STM32 做主机
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;                    // 8-bit 帧
SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;                          // CPOL=1
SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;                          // CPHA=1 (Mode 3)
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;                             // 软件 CS
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_128; // 84MHz/128 = 656kHz
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;                   // 高位先发

// mpu9250.c L8-14
void WriteTo9250(u8 TxData)
{
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET){;}  // 等发送缓冲区空
    SPI_I2S_SendData(SPI1, TxData);                                    // 写入发送数据
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET){;} // 等接收完成
    SPI_I2S_ReceiveData(SPI1);                                         // 读出接收到的数据（丢弃）
}

// mpu9250.c L16-24
u8 ReadToMpu9250(u8 reg)
{
    u8 ReadData = 0;
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET);
    SPI_I2S_SendData(SPI1, reg);                                        // 发寄存器地址
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET);
    ReadData = SPI_I2S_ReceiveData(SPI1);                              // 读返回值
    return(ReadData);
}

// mpu9250.c L26-34
void MPU9250_Write_Reg(u8 reg, u8 value)
{
    GPIO_ResetBits(GPIOC, GPIO_Pin_4);  // CS 拉低，开始传输
    Delay(100);
    WriteTo9250(reg);                   // 发寄存器地址（写操作 MSB=0）
    WriteTo9250(value);                 // 发数据
    GPIO_SetBits(GPIOC, GPIO_Pin_4);    // CS 拉高，结束传输
    Delay(100);
}

// mpu9250.c L36-46
u8 MPU9250_Read_Reg(u8 reg)
{
    u8 val;
    GPIO_ResetBits(GPIOC, GPIO_Pin_4);  // CS 拉低
    Delay(100);
    WriteTo9250(reg | 0x80);            // 寄存器地址 | 0x80 → MSB=1 表示读操作
    val = ReadToMpu9250(0xff);          // 发 0xFF dummy，收回数据
    GPIO_SetBits(GPIOC, GPIO_Pin_4);    // CS 拉高
    Delay(100);
    return val;
}

MPU9250_Write_Reg(INT_PIN_CFG, 0x02);        // Bypass 模式（短暂，用于直接访问）
MPU9250_Write_Reg(56, 0x01);                 // 禁用 I2C aux bypass
MPU9250_Write_Reg(I2C_MST_CTRL, 0x4d);       // I2C 主模式 + 400 kHz 时钟
MPU9250_Write_Reg(USER_CTRL, 0x20);          // 使能 I2C 主控制器 (I2C_MST_EN)
MPU9250_Write_Reg(I2C_MST_DELAY_CTRL, 0x01);// SLV0 延时使能
MPU9250_Write_Reg(I2C_SLV0_CTRL, 0x81);      // 使能 SLV0 + 读 1 字节

// mpu9250.c L54-59  写 AK8963 寄存器
void i2c_Mag_write(u8 reg, u8 value)
{
    MPU9250_Write_Reg(I2C_SLV0_ADDR, MPU9250_AK8963_ADDR);       // AK8963 I2C 地址 (0x0C, 写)
    MPU9250_Write_Reg(I2C_SLV0_REG, reg);                         // AK8963 目标寄存器
    MPU9250_Write_Reg(I2C_SLV0_DO, value);                       // 要写的数据 → MPU6500 自动发起 I2C 写
}

// mpu9250.c L67-76  读 AK8963 寄存器
u8 i2c_Mag_read(u8 reg)
{
    u8 val;
    MPU9250_Write_Reg(I2C_SLV0_ADDR, MPU9250_AK8963_ADDR | 0x80); // AK8963 地址 | 0x80 → 读
    MPU9250_Write_Reg(I2C_SLV0_REG, reg);                          // AK8963 目标寄存器
    MPU9250_Write_Reg(I2C_SLV0_DO, 0xff);                          // dummy
    Delay(20000);                                                   // 等 I2C 传输完成 (~1ms)
    val = MPU9250_Read_Reg(EXT_SENS_DATA_00);                       // 从 MPU6500 读结果
    return val;
}

STM32 ──SPI──► MPU6500 ──内部I2C──► AK8963
                 │
                 └─ 结果存入 EXT_SENS_DATA_00
STM32 ◄─SPI── MPU6500 (读 EXT_SENS_DATA_00)

MPU9250_Write_Reg(PWR_MGMT_1, 0x80);   // DEVICE_RESET = 1，芯片内部复位
Delay(500000);                          // 等复位完成 (~100ms)
MPU9250_Write_Reg(PWR_MGMT_1, 0x01);   // SLEEP=0, CLKSEL=1 (PLL with X axis gyro)

// mpu9250.c L102-112
i2c_Mag_write(AK8963_CNTL2_REG, AK8963_CNTL2_SRST);  // AK8963 软复位
Delay(500000);
i2c_Mag_write(AK8963_CNTL1_REG, 0x16);               // 连续测量模式1 + 16-bit 输出

MAG_x_axis = i2c_Mag_read(AK8963_ASAX);               // 读 X 轴灵敏度修正值
MAG_y_axis = i2c_Mag_read(AK8963_ASAY);               // 读 Y 轴灵敏度修正值
MAG_z_axis = i2c_Mag_read(AK8963_ASAZ);               // 读 Z 轴灵敏度修正值

// mpu9250.c L117-124
MPU9250_Write_Reg(SMPLRT_DIV, 0x0);       // 采样率分频器 = 0 → 1 kHz
MPU9250_Write_Reg(CONFIG, 0x43);           // DLPF_CFG=3 → 带宽 ~41Hz, 内部采样率 1kHz
MPU9250_Write_Reg(GYRO_CONFIG, 0x08);     // GYRO_FS_SEL=01 → ±500°/s
MPU9250_Write_Reg(ACCEL_CONFIG, 0x08);    // ACCEL_FS_SEL=01 → ±4g
MPU9250_Write_Reg(ACCEL_CONFIG_2, 0x48); // ACCEL_FCHOICE_B=01, A_DLPFCFG=0 → 1kHz
MPU9250_Write_Reg(FIFO_EN, 0xf8);        // 使能所有传感器 FIFO

// mpu9250.c L135
while(! GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_15)) {;}  // 等 PA15 = 1 (MPU_INT 数据就绪)

// mpu9250.c L137-148
BUF[0] = MPU9250_Read_Reg(ACCEL_XOUT_L);   // 0x3C 低字节
BUF[1] = MPU9250_Read_Reg(ACCEL_XOUT_H);   // 0x3B 高字节
mpu_AD_value.Accel[0] = (BUF[1]<<8)|BUF[0]; // 大端拼接 → int16
mpu_Data_value.Accel[0] = mpu_AD_value.Accel[0] / 8192.0f;  // 标度转换 → g

// mpu9250.c L150-153
BUF[0] = MPU9250_Read_Reg(TEMP_OUT_L);   // 0x42
BUF[1] = MPU9250_Read_Reg(TEMP_OUT_H);   // 0x41
mpu_AD_value.Temp = (BUF[1]<<8)|BUF[0];
mpu_Data_value.Temp = mpu_AD_value.Temp / 333.87f + 21.0f;  // → °C

// mpu9250.c L155-166
BUF[0] = MPU9250_Read_Reg(GYRO_XOUT_L);   // 0x44
BUF[1] = MPU9250_Read_Reg(GYRO_XOUT_H);   // 0x43
mpu_AD_value.Gyro[0] = (BUF[1]<<8)|BUF[0];
mpu_Data_value.Gyro[0] = mpu_AD_value.Gyro[0] / 65.5f;  // → °/s

void READ_MPU9250_MAG(void)
{
    ST1_temp = i2c_Mag_read(AK8963_ST1_REG);          // 读状态寄存器
    if((ST1_temp & AK8963_ST1_DRDY) == AK8963_ST1_DRDY)  // 数据就绪？
    {
        if(MAG_cnt == 0) {  // 第1次：读 X
            BUF[0] = i2c_Mag_read(MAG_XOUT_L);
            BUF[1] = i2c_Mag_read(MAG_XOUT_H);
            mpu_AD_value.Mag[0] = (BUF[1]<<8)|BUF[0];
            mpu_Data_value.Mag[0] = mpu_AD_value.Mag[0] * 0.25f * (1.0+(MAG_x_axis-128)/256.0f);
            MAG_cnt = 1;
        }
        else if(MAG_cnt == 1) {  // 第2次：读 Y
            // ... 同理读 MAG_YOUT
            MAG_cnt = 2;
        }
        else if(MAG_cnt == 2) {  // 第3次：读 Z
            // ... 同理读 MAG_ZOUT
            MAG_cnt = 0;
        }
        i2c_Mag_write(AK8963_CNTL1_REG, 0x11);  // 触发下一次测量
    }
}

mpu_Data_value.Mag[0] = mpu_AD_value.Mag[0] * 0.25f * (1.0+(MAG_x_axis-128)/256.0f);
//                      ─────────────────  ──────  ──────────────────────────────────
//                      原始 int16 值       标度    ASA 灵敏度修正

i2c_Mag_write(AK8963_CNTL1_REG, 0x11);  // 每次读完一组数据后写 0x11

typedef struct {
    short Accel[3];   // int16 原始 ADC 值
    short Temp;
    short Gyro[3];
    short Mag[3];
} MPU_AD_value;        // ← 原始量（未标度）

typedef struct {
    double Accel[3];  // 已标度（g）
    double Temp;      // 已标度（°C）
    double Gyro[3];   // 已标度（°/s）
    double Mag[3];    // 已标度（μT, 带ASA修正）
    double Pressure;  // MS5611 气压（hPa）
    double Altitude;  // MS5611 高度（m）
} MPU_Data_value;     // ← 物理量（已标度）

MPU_AD_value    mpu_AD_value;    // 原始值
MPU_Data_value  mpu_Data_value;  // 标度后值

wm = (*(CVect3*)mpu_Data_value.Gyro * glv.dps - eb) * TS;
vm = (*(CVect3*)mpu_Data_value.Accel * glv.g0 - db) * TS;
mahony.Update(wm, vm, TS);