关于陀螺仪求解的问题！

1万 · 2019-8-16 14:35

LSM303DLH将上述的加速计、磁力计、A/D转化器及信号条理电路集成在一起，仍然通过I2C总线和处理器通信。这样只用一颗芯片就实现了6轴的数据检测和输出，降低了客户的设计难度，减小了PCB板的占用面积，降低了器件成本。

　　LSM303DLH的典型应用如图10所示。它需要的周边器件很少，连接也很简单，磁力计和加速计各自有一条I2C总线和处理器通信。如果客户的I/O接口电平为1.8V，Vdd_dig_M、Vdd_IO_A和Vdd_I2C_Bus均可接1.8V供电，Vdd使用2.5V以上供电即可；如果客户接口电平为2.6V，除了Vdd_dig_M要求1.8V以外，其他皆可以用2.6V。在上文中提到，LSM303DLH需要置位/复位电路以维持AMR的主磁域。C1和C2为置位/复位电路的外部匹配电容，由于对置位脉冲和复位脉冲有一定的要求，建议用户不要随意修改C1和C2的大小。

1万 · 2019-8-16 14:35

电子指南针主要是通过感知地球磁场的存在来计算磁北极的方向。然而由于地球磁场在一般情况下只有微弱的0.5高斯，而一个普通的手机喇叭当相距2厘米时仍会有大约4高斯的磁场，一个手机马达在相距2厘米时会有大约6高斯的磁场，这一特点使得针对电子设备表面地球磁场的测量很容易受到电子设备本身的干扰。

　　磁场干扰是指由于具有磁性物质或者可以影响局部磁场强度的物质存在，使得磁传感器所放置位置上的地球磁场发生了偏差。如图11所示，在磁传感器的XYZ 坐标系中，绿色的圆表示地球磁场矢量绕z轴圆周转动过程中在XY平面内的投影轨迹，再没有外界任何磁场干扰的情况下，此轨迹将会是一个标准的以O(0,0)为中心的圆。当存在外界磁场干扰的情况时，测量得到的磁场强度矢量α将为该点地球磁场β与干扰磁场γ的矢量和。

1万 · 2019-8-16 14:35

一般可以认为，干扰磁场γ在该点可以视为一个恒定的矢量。有很多因素可以造成磁场的干扰，如摆放在电路板上的马达和喇叭，还有含有铁镍钴等金属的材料如屏蔽罩，螺丝，电阻， LCD背板以及外壳等等。同样根据安培定律有电流通过的导线也会产生磁场，如图12。

1万 · 2019-8-16 14:35

　针对XY轴的校准，将配备有磁传感器的设备在XY平面内自转，如图11，等价于将地球磁场矢量绕着过点O(γx,γy)垂直于XY平面的法线旋转，而红色的圆为磁场矢量在旋转过程中在XY平面内投影的轨迹。这可以找到圆心的位置为((Xmax + Xmin)/2, (Ymax + Ymin)/2). 同样将设备在XZ平面内旋转可以得到地球磁场在XZ平面上的轨迹圆，这可以求出三维空间中的磁场干扰矢量γ(γx, γy, γz).

1万 · 2019-8-16 14:36

经过校准后电子指南针在水平面上已经可以正常使用了。但是更多的时候手机并不是保持水平的，通常它和水平面都有一个夹角。这个夹角会影响航向角的精度，需要通过加速度传感器进行倾斜补偿。

　　对于一个物体在空中的姿态，导航系统里早已有定义，如图17所示，Android中也采用了这个定义。Pitch(Φ)定义为x轴和水平面的夹角，图示方向为正方向；Roll(θ)定义为y轴和水平面的夹角，图示方向为正方向。由Pitch角引起的航向角的误差如图18所示。可以看出，在x轴方向10度的倾斜角就可以引起航向角最大7-8度的误差。

1万 · 2019-8-16 14:36

在当前流行的android 手机中，很多都配备有指南针的功能。为了实现这一功能，只需要配备有ST提供的二合一传感模块LSM303DLH，ST 提供整套解决方案。

1万 · 2019-8-16 14:36

其中包括:

　　BSP Reference

　　Linux Kernel Driver (LSM303DLH_ACC + LSM303DLH_MAG)

　　 HAL Library(Sensors_lsm303dlh + Liblsm303DLH) for sensors.default.so

　　经过library 的计算，上层的应用可以很轻松的运用由Android定义由Library提供的航偏角信息进行应用程序的编写。

1万 · 2019-8-16 14:36

bool hmc5883lSelfTest()
{
  bool testStatus = TRUE;
  int16_t mxp, myp, mzp;  // positive magnetometer measurements
  int16_t mxn, myn, mzn;  // negative magnetometer measurements
  struct
  {
uint8_t configA;
uint8_t configB;
uint8_t mode;
  } regSave;

  // Save register values
  if (i2cdevRead(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)&regSave) == FALSE)
  {
// TODO: error handling
return FALSE;
  }
  // Set gain (sensitivity)
  hmc5883lSetGain(HMC5883L_ST_GAIN);

  // Write CONFIG_A register and do positive test
  i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
   (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
   (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
   (HMC5883L_BIAS_POSITIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));

  hmc5883lSetMode(HMC5883L_MODE_SINGLE);
  vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
  hmc5883lGetHeading(&mxp, &myp, &mzp);

  // Write CONFIG_A register and do negative test
  i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
   (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
   (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
   (HMC5883L_BIAS_NEGATIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));

  hmc5883lSetMode(HMC5883L_MODE_SINGLE);
  vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
  hmc5883lGetHeading(&mxn, &myn, &mzn);

  if (hmc5883lEvaluateSelfTest(HMC5883L_ST_X_MIN, HMC5883L_ST_X_MAX, mxp, "pos X") &&
   hmc5883lEvaluateSelfTest(HMC5883L_ST_Y_MIN, HMC5883L_ST_Y_MAX, myp, "pos Y") &&
   hmc5883lEvaluateSelfTest(HMC5883L_ST_Z_MIN, HMC5883L_ST_Z_MAX, mzp, "pos Z") &&
   hmc5883lEvaluateSelfTest(-HMC5883L_ST_X_MAX, -HMC5883L_ST_X_MIN, mxn, "neg X") &&
   hmc5883lEvaluateSelfTest(-HMC5883L_ST_Y_MAX, -HMC5883L_ST_Y_MIN, myn, "neg Y") &&
   hmc5883lEvaluateSelfTest(-HMC5883L_ST_Z_MAX, -HMC5883L_ST_Z_MIN, mzn, "neg Z"))
  {
DEBUG_PRINT("Self test [OK].n");
  }
  else
  {
testStatus = FALSE;
  }

  // Restore registers
  if (i2cdevWrite(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)&regSave) == FALSE)
  {
// TODO: error handling
return FALSE;
  }

  return testStatus;
}

1万 · 2019-8-16 14:37

//收到一个字节调用一次。把收到的字节做为输入。
void Decode_frame(unsigned char data)
{
if(data==0xa5)
{
      RC_Flag|=b_uart_head; //如果接收到A5 置位帧头标专位
      rx_buffer[rx_wr_index++]=data; //保存这个字节.
      usart_ALL_Printf_En_Dis(RS232_USART,DISABLE,"收到帧头1\r\n");
}
else if(data==0x5a)
{
      if(RC_Flag&b_uart_head) //如果上一个字节是A5 那么认定这个是帧起始字节
      { rx_wr_index=0;  //重置缓冲区指针
         RC_Flag&=~b_rx_over; //这个帧才刚刚开始收
         usart_ALL_Printf_En_Dis(RS232_USART,DISABLE,"收到帧头2\r\n");
      }
      else //上一个字节不是A5
         rx_buffer[rx_wr_index++]=data;
      RC_Flag&=~b_uart_head; //清帧头标志
//       usart_ALL_Printf_En_Dis(RS232_USART,ENABLE,"清帧头标志!\r\n");
}
else
{ rx_buffer[rx_wr_index++]=data;
      RC_Flag&=~b_uart_head;
      if(rx_wr_index==rx_buffer[0]) //收够了字节数.
      {
         RC_Flag|=b_rx_over; //置位接收完整的一帧数据
         AHRS_CommandRoute(); //立即提取数据。
// usart_ALL_Printf_En_Dis(RS232_USART,ENABLE,"立即提取数据!\r\n");
      }
}

if(rx_wr_index==RX_BUFFER_SIZE) //防止缓冲区溢出
      rx_wr_index--;
}

1万 · 2019-8-16 14:38

//--校验当前接收到的一帧数据是否与帧校验字节一致
unsigned char Sum_check(void)
{
unsigned char i;
unsigned int checksum=0;
for(i=0; i<rx_buffer[0]-2; i++)
      checksum+=rx_buffer[i];
if((checksum%256)==rx_buffer[rx_buffer[0]-2])
      return(0x01); //Checksum successful
else
      return(0x00); //Checksum error
}

1万 · 2019-8-16 14:38

//在接收完一帧IMU姿态报告后，调用这个子程序来取出姿态数据
void AHRS_Get_IMU(void)
{
int16_t temp;

temp = 0;
temp = rx_buffer[2];
temp <<= 8;
temp |= rx_buffer[3];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
yaw=(float)temp / 10.0f; //偏航角

temp = 0;
temp = rx_buffer[4];
temp <<= 8;
temp |= rx_buffer[5];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
pitch=(float)temp / 10.0f;//俯仰

temp = 0;
temp = rx_buffer[6];
temp <<= 8;
temp |= rx_buffer[7];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
roll=(float)temp / 10.0f;//滚转

temp = 0;
temp = rx_buffer[8];
temp <<= 8;
temp |= rx_buffer[9];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
alt=(float)temp / 10.0f;//高度

temp = 0;
temp = rx_buffer[10];
temp <<= 8;
temp |= rx_buffer[11];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
tempr=(float)temp / 10.0f;//温度

temp = 0;
temp = rx_buffer[12];
temp <<= 8;
temp |= rx_buffer[13];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
press=(float)temp * 10.0f;//气压

}

1万 · 2019-8-16 14:38

//在接收一帧ReportMotion 后调用这个子程序来取出ADC数据
void AHRS_Get_Motion(void)
{
int16_t temp;

temp = 0;
temp = rx_buffer[2];
temp <<= 8;
temp |= rx_buffer[3];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
ax=temp;//加速度计 X轴的ADC值

temp = 0;
temp = rx_buffer[4];
temp <<= 8;
temp |= rx_buffer[5];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
ay=temp;//加速度计 Y轴的ADC值

temp = 0;
temp = rx_buffer[6];
temp <<= 8;
temp |= rx_buffer[7];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
az=temp;//加速度计 Z轴的ADC值

temp = 0;
temp = rx_buffer[8];
temp <<= 8;
temp |= rx_buffer[9];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
gx=temp;//陀螺仪 X轴的ADC值

temp = 0;
temp = rx_buffer[10];
temp <<= 8;
temp |= rx_buffer[11];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
gy=temp;//陀螺仪 Y轴的ADC值

temp = 0;
temp = rx_buffer[12];
temp <<= 8;
temp |= rx_buffer[13];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
gz=temp;//陀螺仪 Z轴的ADC值

temp = 0;
temp = rx_buffer[14];
temp <<= 8;
temp |= rx_buffer[15];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
hx=temp;//磁力计 X轴的ADC值

temp = 0;
temp = rx_buffer[16];
temp <<= 8;
temp |= rx_buffer[17];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
hy=temp;//磁力计 Y轴的ADC值

temp = 0;
temp = rx_buffer[18];
temp <<= 8;
temp |= rx_buffer[19];
if(temp&0x8000) {
      temp = 0-(temp&0x7fff);
} else temp = (temp&0x7fff);
hz=temp;//磁力计 Z轴的ADC值
}

1万 · 2019-8-16 14:39

//--这个子程序需要在主程序中定时调用,以检查串口是否接收完一帧数据
void AHRS_CommandRoute(void)
{
if(RC_Flag&b_rx_over) { //已经接收完一帧?
      RC_Flag&=~b_rx_over; //清标志先
      if(Sum_check()) {
         //校验通过
         if(rx_buffer[1]==0xA1) { //AHRS_ReportIMU 的数据
            AHRS_Get_IMU(); //取数据
         }
         if(rx_buffer[1]==0xA2) { //AHRS_ReportMotion 的数据
            AHRS_Get_Motion();    //取数据
         }

      }//校验是否通过?
}
}

1万 · 2019-8-16 14:39

void Fix_Dat_result(void)
{
//////////
ax_G=(float)(ax*9.8/13107);
ay_G=(float)(ay*9.8/13107);
az_G=(float)(az*9.8/13107);

}

1万 · 2019-8-16 14:39

在这里添加转换后的数据与三角函数求解，计算加速度分量的纯加速度数据！

1万 · 2019-8-16 14:40

1万 · 2019-8-16 14:40

1万 · 2019-8-16 14:40

1万 · 2019-8-16 14:40

1万 · 2019-8-16 14:40