关于阿波罗mpu9250yaw角度不准问题

SUIJIAN · 发表于 2021-8-21 15:03:24

用的阿波罗F767的板子，单独试9250的程序时没问题，但是现在将dht11、adc、rs485的程序写到一起后yaw角度就不准了，明明转了-90度，结果显示只有-30度，其他的都正常，有无大佬说一下是什么原因啊，怎么解决？

int main(void)
{
u8 t=0;             //Ä¬èÏ¿aÆôéÏ±¨

u16 adcxx;
u8 cangwen;
u8 humidity;
float pitch,roll,yaw;          //Å·à-½Ç
short rspitch,rsroll,rsyaw;                   //ÎÂ¶è
u8 rstxbuf[19];
u8 rsrxbuf[1],ljieshou;
  Cache_Enable();                //′ò¿aL1-Cache
  MPU_Memory_Protection();       //±￡»¤Ïà1Ø′æ′￠Çøóò
  HAL_Init();          //3õê¼»ˉHAL¿a
  Stm32_Clock_Init(432,25,2,9); //éèÖÃê±Öó,216Mhz
  delay_init(216);             //Ñóê±3õê¼»ˉ
uart_init(115200);          //′®¿ú3õê¼»ˉ
usmart_dev.init(108); //3õê¼»ˉUSMAR
LED_Init();       //3õê¼»ˉóëLEDá¬½óμÄó2¼t½ó¿ú
KEY_Init();                   //3õê¼»ˉ°′¼ü
  SDRAM_Init();                //3õê¼»ˉSDRAM
  LCD_Init();                   //LCD3õê¼»ˉ
  MPU9250_Init();          //3õê¼»ˉMPU9250
RS485_Init(9600);          //3õê¼»ˉRS485
MY_ADC_Init();                //3õê¼»ˉADC1í¨μà5
PCF8574_Init();                //3õê¼»ˉPCF8574
PCF8574_ReadBit(BEEP_IO);    //óéóúDHT11oíPCF8574μÄÖD¶Ïòy½Å12óÃò»¸öIO￡¬
                              //ËùòÔÔú3õê¼»ˉDHT11Ö®Ç°òaÏè¶áè¡ò»′&#206

CF8574μÄèÎòaò»¸öIO￡¬
                              //ê1Æäêí·ÅμôÖD¶Ïòy½ÅËùÕ¼óÃμÄIO(PB12òy½Å),·ñÔò3õê¼»ˉDS18B20»á3öÎêìa

  POINT_COLOR=RED;
   while(DHT11_Init()) //DHT113õê¼»ˉ
   {
   LCD_ShowString(30,70,200,16,16,"DHT11 Error");
   }
   while(mpu_dmp_init())
{
   LCD_ShowString(30,150,200,16,16,"MPU9250 Error");
   delay_ms(200);
   LCD_Fill(30,130,239,130+16,WHITE);
   delay_ms(200);
   LED0_Toggle;//DS0éáË¸
}

POINT_COLOR=BLUE;    //éèÖÃ×ÖìåÎaà¶é«
   LCD_ShowString(30,70,200,16,16,"Temp:  C");
   LCD_ShowString(30,90,200,16,16,"Humi:  %");

   LCD_ShowString(30,150,200,16,16,"

itch: . C");
   LCD_ShowString(30,170,200,16,16," Roll: . C");
   LCD_ShowString(30,190,200,16,16," Yaw : . C");
while(1)
{
rstxbuf[1]=rspitch/100+'0';
rstxbuf[2]=(rspitch%100)/10+'0';
rstxbuf[3]=rspitch%10+'0';
rstxbuf[5]=rsroll/1000+'0';
rstxbuf[6]=(rsroll%1000)/100+'0';
rstxbuf[7]=(rsroll%100)/10+'0';
rstxbuf[8]=rsroll%10+'0';
rstxbuf[10]=rsyaw/1000+'0';
rstxbuf[11]=(rsyaw%1000)/100+'0';
rstxbuf[12]=(rsyaw%100)/10+'0';
rstxbuf[13]=rsyaw%10+'0';
rstxbuf[15]=cangwen/10+'0';
rstxbuf[16]=cangwen%10+'0';
rstxbuf[17]=humidity/10+'0';
rstxbuf[18]=humidity%10+'0';

   if(mpu_mpl_get_data(&pitch,&roll,&yaw)==0)
   {}

if((t%10)==0)
{

rspitch=pitch*10;
if(rspitch<0)
{
rstxbuf[0]=1+'0';
LCD_ShowChar(30+48,150,'-',16,0); //ÏÔê¾¸ooÅ
rspitch=-rspitch; //×aÎaÕyêy
}else {rstxbuf[0]=0+'0';LCD_ShowChar(30+48,150,' ',16,0);} //è￥μô¸ooÅ
LCD_ShowNum(30+48+8,150,rspitch/10,3,16); //ÏÔê¾Õûêy2¿·Ö
LCD_ShowNum(30+48+40,150,rspitch%10,1,16); //ÏÔê¾D¡êy2¿·Ö
rsroll=roll*10;
if(rsroll<0)
{
rstxbuf[4]=1+'0';
LCD_ShowChar(30+48,170,'-',16,0); //ÏÔê¾¸ooÅ
rsroll=-rsroll; //×aÎaÕyêy
}else {rstxbuf[4]=0+'0';LCD_ShowChar(30+48,170,' ',16,0);} //è￥μô¸ooÅ
LCD_ShowNum(30+48+8,170,rsroll/10,3,16); //ÏÔê¾Õûêy2¿·Ö
LCD_ShowNum(30+48+40,170,rsroll%10,1,16); //ÏÔê¾D¡êy2¿·Ö
rsyaw=yaw*10;
if(rsyaw<0)
{
rstxbuf[9]=1+'0';
LCD_ShowChar(30+48,190,'-',16,0); //ÏÔê¾¸ooÅ
rsyaw=-rsyaw; //×aÎaÕyêy
}else {rstxbuf[9]=0+'0';LCD_ShowChar(30+48,190,' ',16,0); } //è￥μô¸ooÅ
LCD_ShowNum(30+48+8,190,rsyaw/10,3,16); //ÏÔê¾Õûêy2¿·Ö
LCD_ShowNum(30+48+40,190,rsyaw%10,1,16); //ÏÔê¾D¡êy2¿·Ö
      adcxx=Get_Adc_Average(ADC_CHANNEL_5,3);//»ñè¡í¨μà5μÄ×a»»Öμ￡¬20′Îè¡Æ½¾ù
if(adcxx>1200)
      {
rstxbuf[14]=1+'0';
   LCD_ShowString(30,50,200,16,16,"da");
      }
   else
      {
rstxbuf[14]=0+'0';
   LCD_ShowString(30,50,200,16,16,"xiao");
   }
PCF8574_ReadBit(BEEP_IO);
DHT11_Read_Data(&cangwen,&humidity); //¶áè¡ÎÂêa¶èÖμ
//MS5837_Getdata(); //»ñè¡′óÆøÑ1
//
LCD_ShowNum(30+40,70,cangwen,2,16); //ÏÔê¾ÎÂ¶è
      LCD_ShowNum(30+40,90,humidity,2,16); //ÏÔê¾êa¶è

RS485_Send_Data(rstxbuf,19);
t=0;
}

      t++;
RS485_Receive_Data(rsrxbuf,&ljieshou);
if(ljieshou)
      {
   if(rsrxbuf[0]==48)
      {
      LED0_Toggle;
      }
   if(rsrxbuf[0]==99)
      {
      LED1_Toggle;
      }
}//èç1û½óêÕμ½êy¾Y
}
}