ジャイロセンサーの繰り返し誤差の補正について
マイコン初心者です。
SH7144のA/Dコンバーターにジャイロセンサーを接続し、ジャイロの出力を積分して、角度を求めようとしました。下記のコードで、積分値は出せたのですが、ジャイロを一度傾けて元に戻しても初期値に戻らず、オフセットが残ってしまいます。積分前の出力を見ると、CW側にジャイロを回すと、CW側に値が振れた後にCCW側へ少し値が振れているようなので、角速度のデータから来る誤差じゃないかと自分では仮説を立ててみたものの、その補正のところでつまづいてしまいました。
質問はCだけの問題ではないかもしれませんが、組み込みでジャイロセンサーを扱った経験のある方がいましたら、ご教授をお願いいたします。
ジャイロセンサーは村田製作所製圧電振動ジャイロ、ジャイロスターを使用し、カタログに載っているハイパスフィルタとローパスフィルタを通して、SH7144のA/Dコンバーターへ接続しています。
・ジャイロスターのカタログ
ttp://www.murata.co.jp/catalog/s42.pdf
角速度はコンペアマッチタイマを使用し、ノイズ対策のためにCMT0で角速度の平均値を計算しています。CMT0は0.8msecごとにで割り込みが発生するように設定し、main()ではタイマのカウンタを開始するコードのみを書いています。下記にCMT0割り込みのコードを示します。
void int_cmi0(void)
{
unsigned static int t_ave = 0;
static short int adc0_buffer[3] = {0, 0, 0};
unsigned short int i;
unsigned static short int count0 = 1;
adc0_buffer[0] = adc0_buffer[0] + AD0.ADDR0.BYTE;
adc0_buffer[1] = adc0_buffer[1] + AD0.ADDR1.BYTE;
adc0_buffer[2] = adc0_buffer[2] + AD0.ADDR2.BYTE;
t_ave++;
if( t_ave > 100) //AD0のADDR0～2の100回平均を求めています
{
for(i = 0; i < 3; i ++)
{
data_adc0[i] = (adc0_buffer[i] / t_ave) - 109; //"109"は静止時の出力電圧を示しています
if(-3 <= data_adc0[i] && data_adc0[i] <= 3) //平均しても振れてしまう値の分をカットしています
{
data_adc0[i] = 0;
}
adc0_buffer[i] = 0;
if(count0 > 89) //電源投入時にdata_adc0[]の値が安定しないため、最初の10個のデータを捨てています。
{
count0 = 100;
}
else
{
count0++;
data_adc0[i] = 0;
}
}
t_ave = 0;
}
CMT0.CMCSR.BIT.CMF = 0; //コンペアマッチフラグをクリア
}
角度は、CMT1を使用し、8msecの間隔で割り込みを発生させ計算しています。最終的な計算結果は、SH7144のSCIを使用して、PCのハイパーターミナルに表示しています。
void int_cmi1(void)
{
static short int adc0_buffer_s[3] = {0, 0, 0}; //角速度の初期値を入れるバッファです
unsigned static short int count1 = 1;
short int i;
for(i = 0; i < 3; i ++)
{
data_adc0i[i] =data_adc0i[i] + ((adc0_buffer_s[i] + data_adc0[i]) / 2); //積分を行う部分です。最終的に制御用のゲインにしようと考えているので、時間項8msecは明記していないです。
adc0_buffer_s[i] = data_adc0[i];
if(count1 > 89) //角速度と同様最初の10個のデータは0にしています
{
count1 = 100;
}
else
{
count1++;
data_adc0i[i] = 0;
}
}
CMT1.CMCSR.BIT.CMF = 0; //コンペアマッチフラグをクリア
}
main()では、各機能のイニシャライズと各CMTのカウンタ動作を開始するコードのみを書いています。ハイパーターミナルで動作が見れているので、各機能は動作していると思います。
void main(void)
{
initIO(); //各機能のイニシャライズです。
initINT();
initSCI();
initADC();
initCMT();
CMT.CMSTR.BIT.STR0 = 1; //CMT0カウント開始
CMT.CMSTR.BIT.STR1 = 1; //CMT1カウント開始
while(1);
}
![その他([技術者向] コンピューター) イメージ](https://gazo.okwave.jp/okwave/spn/images/related_qa/c205_4_thumbnail_img_sm.png)
お礼
もっともなご意見ですよね。ありがとうございます。 どちらを使用するか検討してみます。