(原文件名:傳感器融合1、圖中實線部分為實時計算獲取傾角值，虛線部分是計算陀螺儀偏差。s1、s20.5s21000，90030.5s12ms（2、獲取傳感器數(shù)34、5Self-balancing*By#include //FPGANIOSII#include"sys/alt_irq.h"#include"sys/alt_dev.h"#include"alt_types.h"#include"al_avalon_pio_regs.h"#include"al#include"al #include"stdio.h"alt_u8data_acc; //readfromadalt_u8data_gyr;alt_u8 //Getdegree AuttitudecorrectionRoll=0; staticintinttimer_periodh,timer_periodl;intbalance_ctrl=1;intrun=floatAmplify=floatProgramset_GyrAcc=#defineMIDDLE_ACC#defineP#defineI#defineDvoid //初始化intalt_u16}middle_gyr=}void nios2-terminalstatic JAVAGUI }}staticvoidInterrupt_timer(void*context,alt_u32id){ balance_ctrl=1;IOWR_AL}void }intalt_u8 //low-passalt_u8data_gyr_old; intintegral;intinttimer_periodh= //2mstimer_periodl=0x86A0; balance_ctrl= //由IP核傳感器/*LOWPASSdata_acc=data_acc*71/100+data_acc_old*29/100;data_gyr=data_gyr*71/100+data_gyr_old*29/100;data_acc_old=data_acc;data_gyr_old=/*Getsensors'valuewhichhassubstrictedbyMiddlevalue*/Reading_AccRoll=data_acc-MIDDLE_ACC;Reading_GyrRoll=data_gyr-middle_gyr; Renew Mean_AccRoll=(Mean_AccRoll+IntegralAccRoll+= //Beusedforlong-timeReading_IntegralGyrRoll+=(Reading_GyrRoll-AuttitudecorrectionRoll);//Real-timecontrolReading_IntegralGyrRoll_2+=Reading_GyrRoll;//OriginaldataintegralIntegralGyrRoll=Reading_IntegralGyrRoll; //Outputtilt-RollIntegralGyrRoll_2=Reading_IntegralGyrRoll_2;Mean_IntegralGyrRoll+= //Long-time Real-time Correction_Roll=((IntegralGyrRoll/Programset_GyrAcc)-Mean_AccRoll);//ErrorsignalCorrection_Roll/=2;#defineMaxCorrectionif(Correction_Roll>= Correction_Roll=elseif(Correction_Roll<=-MaxCorrection) Correction_Roll=-MaxCorrection;Reading_IntegralGyrRoll-=Correction_Roll; Long-time #defineBAL_NUM250if(balance_number>= {alt_16Mean_IntegralGyrRoll/=BAL_NUM;IntegralAccRoll/=BAL_NUM;long_correct=(Mean_IntegralGyrRoll-IntegralAccRoll);AuttitudecorrectionRoll=(long_correct/BAL_NUM);IntegralAccRoll=//printf("AuttitudecorrectionRoll%d//printf("AuttitudecorrectionNick%d 修正陀螺儀溫 IntegralErrorRoll=IntegralGyrRoll_2-IntegralGyrRoll;Reading_IntegralGyrRoll_2-=IntegralErrorRoll;if(IntegralErrorRoll>=4*balance_number)middle_gyr+=1;if(IntegralErrorRoll<=-4*balance_number)middle_gyr-=balance_number=}

degree= if(integral<-4999)integral=-49