JP2011503679A - ロール補償を提供する方法及び装置 - Google Patents

Abstract

本開示の実施形態は制御装置（１１０）においてロール補償を提供する方法及び装置に向けられ、前記方法及び装置は前記制御装置（１１０）の動きを示す回転データ及び直線データを獲得し（３１０）、その獲得したデータにロール補償を適用し（３２０）、そのロール補償したデータからロール補償エラーを除去する（３３０）ことを含む。回転データ及び直線データを獲得するためにジャイロスコープセンサ（２３０，２４０）及び加速度計センサ（２５０）等の慣性センサを使用することができる。

Description

本出願は２００７年９月２６日に合衆国に提出された仮出願６０／９９５３８２の３５Ｕ．Ｓ．Ｃ．§１１９に基づく利益を主張する。

本発明は、一般的なハンドヘルド制御装置に関し、特にロール補償及びそれと関連した使い勝手の良さを有するハンドヘルド制御装置に関する。

この欄は、以下に説明及び／又は請求される本発明の様々な態様に関連され得る技術の様々な態様に読み手を案内するべく意図されている。この議論は、本発明の様々な態様のより良い理解を容易にするために背景情報を読み手に提供する助けとなると信じられる。よって、それらの記述は、従来技術の承認としてではなく、この観点から読まれるべきである。

従来の制御装置はテレビ受像機、コンピュータ、及び表示装置等の受信機を制御する。典型的な制御機能は受信機の電源のオン及びオフ、受信機により表示されるメニューを介した案内、並びに、受信機により表示されるグラフィカル要素の表示の制御を含む。通常、ユーザは制御装置のボタン（例えば、電源ボタン、案内ボタン、メニューボタン等）を用いてそれらの制御機能をアクセスし、或いは、制御装置は単純な動き（例えば、表面に沿った動きを検出するためのトラックボール又はＬＥＤ配列を用いたマウス）を検出することができる。それらの従来の制御装置には自由空間におけるそれら自身の動きを追わないという欠点がある（すなわち、従来の制御装置は表面に沿ったそれらの動きをたどることだけができる）。

近時、最新の制御装置が紹介されている。それらの最新の装置は加速度計センサ、ジャイロスコープセンサ等の慣性センサを用いて自由空間でそれらの動きを追うことができる。しかしながら、それらの最新の制御装置にはロール補償を提供しないという欠点がある。その結果、最新の制御装置の自由空間の追跡は正確ではない。

本開示はそれらの欠点を解決することに向けられている。

本発明の開示した実施形態は制御装置においてロール補償を提供する方法及び装置に向けられ、その方法及び装置は制御装置の動きを示す回転データ及び直線データを獲得し、その獲得したデータにロール補償を適用し、ロール補償したデータからロール補償エラーを除去することを含む。加速度計センサ、ジャイロスコープセンサ等の慣性センサは回転データ及び直線データを獲得するために用いることができる。

本開示の実施形態を用いる典型的な制御システム例のブロック図である。 図１の動き検出プロセッサを更に示す図である。 本開示の典型的なロール補償処理例を示すブロック図である。

本発明の１以上の特定の実施形態は以下に説明される。それらの実施形態の簡潔な説明を提供する目的においては、実際の実施の全ての特徴が明細書に説明されるとは限らない。なお、エンジニアリングプロジェクト又は設計プロジェクトにおいてのように、実際の実施の開発では、実施毎に変わるかもしれない、システムと関連しかつビジネスと関連した制限に合わせた開発者の特定の目標に多くの実施特定決定が到達するようにされなければならない。更に、そのような開発の努力は複雑で時間消費であるが、それでも本開示の利益を有する当業者のための設計、組み立て及び製造を行うルーチンであることが理解されよう。

次は、加速度計、ジャイロスコープ等の慣性センサを有する制御装置を用いて表示装置を制御するシステムを説明する。表示装置以外の装置を制御するために用いられる他のシステムは非常に類似の構成を含むことができる。本技術分野の当業者は、本明細書に説明されたシステム及び回路の実施形態が単に１つの可能性がある実施形態であると評価するであろう。そのようなものとして、代替的実施形態においてはシステム及び回路のコンポーネントを再配置又は除外することができ、或いは、追加のコンポーネントを追加することができる。

次に図面を使用し、先ず図１を参照すると、本開示の実施形態を用いるシステム例のブロック図が示される。図１は、可能性がある応用、環境又はシステム１００を示し、本開示のロール補償処理を利用することができる。システム１００は、制御装置１１０と、受信機１２０とを含んでいる。制御装置１１０は、限定されないが、リモート制御装置、マウス、ゲームコントローラ、仮想現実コントローラ、ポインタ、モバイル装置（例えば、ラップトップコンピュータ、ＰＤＡ、携帯電話機、無線電話機、モバイルＴＶ等）、又は他のディジタルメディア入力装置であることができる。制御装置１１０は後述するように、移動されたとき（すなわち、Ｘ平面，Ｙ平面又はＺ平面に沿って直線的に移動されたとき、回転されたとき（例えば、ピッチ、ヨー及びロール動き）、又はそれらの組み合わせ）、検出し、その動きの検出に応答して制御装置１１０は制御信号を受信機１２０に送信する。受信機１２０は、限定されないが、テレビ、表示装置、セットトップボックス、ゲームコンソール、仮想現実コンソール等であることができる。更に、受信機１２０及び制御装置１１０は、プラットフォームが移動されたとき制御装置によって制御信号が発生されるように同一のプラットフォーム（例えば、ハンドヘルドゲームコンソール、ＰＤＡ、携帯電話機、無線電話機、モバイルテレビＴＶ等）に統合されても良いことが注意されるべきである。制御装置１１０からの制御信号の受信機１２０の受信に応答して、受信機１２０は、限定されないが、カーソル、ハイライトボックス、メニュー、ゲームキャラクタ又は対象物等のグラフィカル要素を表示装置上で移動させることができる。グラフィカル要素の移動の精度を詳細に後述されるように本開示のロール補償処理によって改善することができる。受信機は、移動可能な装置（例えば、リモート制御自動車、飛行機、ボート、電気機械装置等）に統合されて制御装置１１０の動きが移動可能な装置の動きとなるようにしても良いことが注意されるべきである。

制御装置１１０は、マイクロプロセッサ１３０、送信機１４０、ユーザインターフェース１５０、及び本開示のロール補償処理を利用する動き検出プロセッサ１６０を含んでいる。１つの実施形態においては、マイクロプロセッサはユーザインターフェース１５０及び動き検出プロセッサ１６０からの入力を受信し、その入力に基づいて送信機１４０に制御信号を受信機１２０に対して送信させる。代替的実施形態においては、例えば、受信機１２０及び制御装置１１０が同一のプラットフォームに統合されるとき、マイクロプロセッサ１３０が表示装置上のグラフィカル要素の動きを直接制御しても良く、又は受信機１２０、他の中間モジュール又は回路、或いはそれらの組み合わせを介してそのような動きを間接的に制御しても良い。ユーザインターフェース１５０は、限定されないが、ユーザ選択可能ボタン、スイッチ、ダイアル、スクロール又はマウスホイール、タッチ検出スクリーン等を含むことができる。ユーザインターフェース１５０によって制御又は初期化されるいくつかの機能例は、限定されないが、制御装置の電源をオンオフすること、表示装置上のハイライトの対象物を選択すること、動き検出プロセッサ１６０の動作を調整すること（例えば、制御装置１１０が表面にあるならば動き検出プロセッサ１６０を第１モードにさせ、制御装置１００が表面から除去され、自由空間に移動されているならば、動き検出プロセッサ１６０を第２モードにさせる）、メニューを表示装置に表示させること、又はその技術分野の当業者によって知られた他の機能を含むことができる。動き検出プロセッサ１６０は後述するように、様々な動きセンサから動きデータ又は情報を獲得し、その動きデータを組み合わせ又は統合して制御装置１１０の動きを示す動き信号を生成する。その動き検出プロセッサ１６０は、マイクロプロセッサ１３０が例えば、グラフィカル対象物を表示装置上で移動させることができるように、その動き信号をマイクロプロセッサ１３０に供給する。

好ましい実施形態においては、制御装置１１０は次の特徴を含んでいる。制御装置１１０は５つの自由度（例えば、動きがジャイロスコープセンサ等の慣性センサによって２つの回転軸に沿って、そして加速度計センサ等の慣性センサによって３つの直線軸に沿って追跡される）を有する。制御装置１１０は、回転した方向がＸ−Ｙ動きを生成するように３６０度のロール補償を有する。制御装置１１０は例えば、加速度計が制御装置１１０の動きの間にドリフト補償を促進又は許可するように、動的ドリフト補償を有する。制御装置１１０は小サイズ、低電力消費、速い起動時間を有している。制御装置１１０の動作パラメータの例は、直流２．７〜３．３ボルト（ＶＤＣ）の供給電圧を有すること、マイクロプロセッサ１３０と動き検出プロセッサ１３０との間にシリアル・ペリフェラル・インターフェース(ＳＰＩ：Serial Peripheral Interface)を有すること、７マイクロアンペアの検出動き電流、４．５ミリアンペアの定常電流、及び８ミリ秒の最大サンプリングレートを有することを含む。

図２を参照すると、図１の動き検出プロセッサ１６０がより詳細に示されている。動き検出プロセッサ１６０は、アナログ調整回路２２０及びアナログ−ディジタル変換器（ＡＤＣ）２１０を介してジャイロスコープセンサ２３０及び２４０等の複数の慣性センサから回転又は角度動き信号を受信するコントローラ２００を含んでいる。アナログ調整回路２２０はジャイロスコープセンサ２３０及び２４０から受信されたアナログ回転動き信号をフィルタリング処理し、それによりそのフィルタリング処理されたアナログ回転動き信号がＡＤＣ２１０によってディジタル化されて適切な状態になる。アナログ信号を調整する様々なフィルタ及びフィルタリング技術はこの技術分野の当業者には公知であり、本開示の範囲内のことと考慮される。ＡＤＣ２１０は回転動き信号をディジタル化し、そのディジタル化回転動き信号をコントローラ２００に供給する。他のＡＤＣ分解能が本開示の範囲内のことと考慮されるが、ＡＤＣ２１０は好ましくは１０ビットで２チャンネルＡＤＣ分解能を有している。

また、コントローラ２００は加速度計センサ２５０等の複数の慣性センサから直線動き信号を受信する。１つの実施形態の例においては、３つの加速度計センサ２５０がコントローラ２００に信号を供給する。第１の加速度計センサはＸ軸に沿った動きを検出し、そのＸ軸に沿った動きを示す信号をコントローラ２００に供給し、第２の加速度計センサはＹ軸に沿った動きを検出し、そのＹ軸に沿った動きを示す信号をコントローラ２００に供給し、第３の加速度計センサはＺ軸に沿った動きを検出し、そのＺ軸に沿った動きを示す信号をコントローラ２００に供給する。アナログ又はディジタル信号又はデータが加速度計センサ２５０によって提供されれば良く、コントローラ２００によるいずれかの処理は本開示の範囲内のことと考慮されることが注意されるべきである。代替的実施形態においては、Ｘ軸，Ｙ軸及びＺ軸に沿った制御装置の直線的な動きを検出するために３軸加速度センサ等のマイクロ電気機械システム（ＭＥＭＳ：Micro Electro-Mechanical System）デバイスを使用することができる。

コントローラ２００は好ましくは、詳細に後述するように本開示によるロール補償を提供する処理を含む例えば、ファームウエア内にソフトウエアモジュールを含んでいる。そのソフトウエアモジュールは、代替的実施形態においては、コントローラ２００の内部メモリ又は外部メモリ（図示せず）に保存できることが注意されるべきである。ロール補償処理は参照のフレームを変換することなく受信した回転レートデータについて直接ロール補償を行うことを含み、更に、制御装置１１０の遠心加速度及び直線加速度を計算することなくロール補償の望ましくない結果を除去することを含む。

コントローラ２００は、次の動作パラメータ、供給電圧範囲２．７ＶＤＣ（直流電圧）〜５．５ＶＤＣで動作し、温度範囲−１０℃（摂氏）〜＋７０℃で動作し、３．３ＶＤＣ及び２５℃で１．７ミリアンペアの標準的電流消費を有し、１０マイクロアンペアのスリープ電流消費を有する集積回路として構成されても良い。好ましくは、コントローラ２００は、限定されないが、ゲイン、閾値、及び電力状態制御等のいくつかのパラメータの動的調整を考慮に入れている。

ＡＤＣ２１０、アナログ調整回路２２０、ジャイロスコープセンサ２３０及び２４０、並びに加速度計センサ２５０はコントローラ２００から分離しているとして示されているが、それらの要素のいずれか又は全てがコントローラ２００の欠かせない一部であっても良いことが注意されるべきである。そのような統合したコントローラを製作又は構成することはその技術分野の当業者には公知であり、本開示の範囲内のことと考慮される。

コントローラ２００はジャイロスコープセンサ２３０及び２４０、並びに加速度計センサ２５０によって生成された直線動き信号及び回転動き信号を受信し、その動き信号を処理して特に本開示によるロール補償を提供し、制御装置１１０のマイクロプロセッサ１３０に処理した動き信号を供給する。コントローラ２００はその処理した回転動き信号及び直線動き信号各々を個別にマイクロプロセッサ１３０に供給しても良く、その処理した回転動き信号及び直線動き信号を表す統合信号をマイクロプロセッサ１３０に供給しても良く、或いは、例えば、制御装置が動作している動作モードに応じてその処理した回転動き信号及び直線動き信号のいくつかをマイクロプロセッサ１３０に選択的に供給するだけでも良いことが注意されるべきである。例えば、制御装置１１０がデスクトップマウスとして動作しているならば、コントローラ２００はＸ方向及びＹ方向への制御装置の直線移動を示す信号を供給するだけでも良い。或いは、制御装置１１０が自由空間で動作しているならば、コントローラは制御装置の直線動き及び回転動きの全てを示す個別の信号又は統合信号を供給しても良い。

ここで図３を参照すると、本開示によるロール補償を提供する方法又は処理３００が示されている。先ず、ステップ３１０にて、コントローラ２００は上述したように、ジャイロスコープセンサ２３０及び２４０から回転動きデータ又は信号、並びに加速度計センサ２５０から直線動きデータ又は信号を受信又は獲得する。次に、ステップ３２０にて、コントローラ２００は参照のフレームを変換することなくその動きデータについてロール補償を提供又は適用する。そのロール補償ステップ３２０は以下に提供されたソフトウエアモジュールの説明で更に論じられる。その後、ステップ３３０にて、コントローラ２００は、以下に提供されたソフトウエアモジュールの説明で更に論じられるように、制御装置１１０の遠心加速度及び直線加速度を計算することなく、ロール補償の適用の望ましくない結果又はロール補償エラーを動きデータから除去する。

本開示によるロール補償を提供する方法は以下に提供されたソフトウエアモジュールで論じられる。ソフトウエアモジュールはコントローラ２００によって利用されるファームウエアの一部である。
コントローラ２００のロール補償特徴を実施するソフトウエアモジュール：
***************************************************************/
外部宣言ファイルの包含(Inclusion of External Declaration Files)
***************************************************************/
#include "project_config.h"
#include <stdio.h>
#include <stdlib.h>
#include "spi.h"
//-------------------------------
// External Function Declarations
//-------------------------------
void Trap (unsigned char);
//-------------------------------
// Local Function Declarations
//-------------------------------
double Dabs(double x);
double Swine(double x);
double Root(double r);
double Get_Theta( double x, double y);
void Gyro_Off (void);
//-------------------------------
// Local Definitions
//-------------------------------
#define PI 3.141592654
#define PIBY2 1.5707963
#define COEFF_1 0.7854
#define COEFF_2 2.3562
#define COEFF_3 1.27323954474
#define COEFF_4 -0.4052847346
#define COEFF_5 0.225
#define READ_X 0x93
#define READ_Y 0xB3

#define ACTIVATE_INTERVAL 5
#define DESK_SENSE_INTERVAL 100
#define DESK_SENSE_PULSE 500
#define ADC_INTERVAL 6
#define GYRO_TIMER_INTERVAL 100
#define GYRO_SCALE_FACTOR 50
#define STEADY_THRESHOLD 20
#define MOTION_NOISE 0 //wmm 10 > 0 for testing
#define STEADY_SAMPLES 10
//#define CT 374//wmm 374 is theoretical center
//#define SPAN 8
#define GUL 406 //wmm G Vector Magnitude qualifier
Upper Limit +16VG
#define GLL 342 //wmm G Vector Magnitude qualifier
Lower Limit +16VG
// CT = 374.374 +/- 64 = 438,310
// CT = 374.374 +/- 32 = 406,342 proto values
// CT = 374.374 +/- 16 = 390,358
// CT = 374.374 +/- 8 = 382,366 = RC too small
#define GVECTOR_ZERO 3.0 //wmm 10*sqrt(Gain_AX)= ~ 15 deg.
/***************************************************************
* 変数記憶（Variable Storage）
****************************************************************/
//extern unsigned char self_steady_done_flag;
unsigned int rest_count_x, rest_count_y;
extern unsigned char Buf[];
signed int gyro_x, gyro_y;
signed int x_min, y_min, x_max, y_max;
signed int ax_min, ay_min, az_min, ax_max, ay_max, az_max;
signed long x_steady, y_steady;
unsigned char num_steady_samples;
signed int x_mickeys, y_mickeys;
unsigned long gyro_timer;
unsigned char turned_on;
signed int accel_x, accel_y, accel_z, gain_sum, sign;
signed int x sum, y_sum, z_sum;
double dax, day, daz;
double theta2, theta3, theta4;
double rrx, rry, rr_mag, gvisya;
double grv_axyz_mag;
double extra_yaw, extra_pitch;
extern unsigned char gyro_state;
//-------------------------------------------
// Gyro Support Routines
//-------------------------------------------
//-------------------------------------------
// Gyro_lnit
//-------------------------------------------
void Gyro_lnit(void)
{
ADC12CTL1 = (AdcClkDiv << 5) | (AdcClkSel << 3) | (CONSEQ0);
ADC12MCTL0 = 0x70;// 0x7x uses AVex as the reference so we are quiter
and temp stability is increased.
ADC12MCTL1 = 0x71 | EOS;
theta3 = 0;//wmm
// grv_axyz_mag_ref = 68;
}
//-------------------------------------------
// Read_XYZ
//-------------------------------------------
void Read_XYZ(void)
{
accel_x = spi_xyz_get (OUT_X_L);
accel_x |= (spi_xyz_get (OUT_X_H)) << 8;
accel y = spi_xyz_get (OUT_Y_L);
accel_y |= (spi_xyz_get (OUT_Y_H)) << 8;
accel_z = spi_xyz_get (OUT_Z_L);
accel_z |= (spi_xyz_get (OUT_Z_H)) << 8;
P2OUT ^= 1;
}
//-------------------------------------------
// Read_ADC
//-------------------------------------------
void Read_ADC(void)
{
gyro_x = Oxfff - ADC12MEM0;
gyro_y = Oxfff - ADC12MEM1;
ADC12CTL0 &= ~(ENC | ADC12SC); // Set the X enable and start conversion
sequence
asm ("dint");
Read_XYZ();
asm ("eint");
}
//-------------------------------------------
// Start_ADC
//-------------------------------------------
void Start_ADC(void)
{
if (gyro_timer)
{
ADC12CTL0 |= ENC | ADC12SC; // Set the X
enable and start conversion sequence
QueDelay (Read_ADC, 1 ); // Queue task to
read the results
}
}

void ClearWarmup (void)
{
if (STEADY_FLAG_TRUE)
{
SelfSteadyCounter--;
if (!SelfSteadyCounter)
{
CLR_STEADY_FLAG;
if (ProgramMode)
lcd_print (LEFT, 6, 1 , "Self Steady failed", 0, 0);
Gyro_Off();
}
}
CLR_WARMUP_FLAG;
}
//-------------------------------------------
// Gyro_On
//-------------------------------------------
void Gyro_On(void)
{
if (!gyro_timer)
{
P6DIR &= ~3;
P6OUT &= ~0x80; // Apply power to gyros and A/D
ADC12CTL0 = ADC12ON | (AdcRef << 6) | REFON | (SampleTime <<
8) | (AdcMulti << 7);
P6SEL |= 3; // Set the
port mode select bits
spi_xyz_put (CTRL1, POWUP | DEC0 | XENA | YENA | ZENA);
spi_xyz_put (CTRL2, MAX6G | BDU);
SampleDivider = 17;
SET_WARMUP_FLAG;
SET_SAMPLE_FLAG;
QueDelay (ClearWarmup, 250);
gyro_timer = GYRO_RUN_TIME;
}
}

//-------------------------------------------
// Gyro_Off
//-------------------------------------------
void Gyro_Off(void)
{
CLR_SAMPLE_FLAG;
spi_xyz_put(CTRL1 , 0);
ADC12CTL0 = 0;
P6SEL &= ~3; // Deactivate the A/D input pins
P6OUT |= 0x80; // Remove power from gyros and A/D
gyro_timer = 0;
}

//-------------------------------------------
// Calc_Mickeys
//-------------------------------------------
void Calc_Mickeys(void)
{
x_mickeys = ((gyro_x - x_offset));
y_mickeys = ((gyro_y - y_offset));

if((x_mickeys < G_MotionNoise)&&(x_mickeys > -G_MotionNoise) &&\
(y_mickeys < G_MotionNoise)&&(y_mickeys > -G_MotionNoise))
x_mickeys = y_mickeys = 0;

if(x_mickeys > (MaxMickeys * Gain_X))//wmm can't find where we limit to
maxmickeys so I added...
x_mickeys = MaxMickeys * Gain_X;
if(x_mickeys < (-MaxMickeys * Gain_X))
x_mickeys = -MaxMickeys * Gain_X;
if(y_mickeys > (MaxMickeys * Gain_Y))
y_mickeys = MaxMickeys * Gain_Y;
if(y_mickeys < (-MaxMickeys * Gain_Y))
y_mickeys = -MaxMickeys * Gain_Y;

}

//-------------------------------------------
// Reset_Self_Steady
//-------------------------------------------
void Reset_Self_Steady(void)
{
x_min = gyro_x;
y_min = gyro_y;
x_max = gyro_x;
y_max = gyro_y;
ax_min = accel_x;
ax_max = accel x;
ay_min = accel_y;
ay_max = accel_y;
az_min = accel_z;
az_max = accel z;
x_sum = accel_x;
y_sum = accel_y;
z_sum = accel_z;
x_steady = gyro_x;
y_steady = gyro y;
num_steady_samples = STEADY_SAMPLES - 1;
}

//-------------------------------------------
// Self_Steady
//-------------------------------------------
void Self_Steady(void)
{
x_steady += gyro_x;
y_steady += gyro_y;
x_sum += accel_x;
y_sum += accel_y;
z_sum += accel_z;
if (gyro_x > x_max) /* Adjust max & min values */
x_max = gyro_x;
if (gyro_x < x_min)
x_min = gyro_x;
if (gyro_y > y_max)
y_max = gyro_y;
if (gyro_y < y_min)
y_min = gyro_y;
if (((ax_max - ax_min) > A_SteadyThreshold) ||
((ay_max - ay_min) > A_SteadyThreshold) ||
((az_max - az_min) > A_SteadyThreshold))
{
SET_WARMUP_FLAG;
QueDelay (ClearWarmup, 1000);
sprintf (Buf, "Noise Z%c", 0x1c);
if (ProgramMode)
lcd_print(LEFT, 5, 1, Buf, 0, 0);
}
else
if ((x_max - x_min) > X_SteadyThreshold)
{
SET_WARMUP_FLAG;
QueDelay (ClearWarmup, 1000);
sprintf (Buf, "Noise X max:%x min:%x", x_max, x_min);
if (ProgramMode)
lcd_print (LEFT, 5, 1, Buf, 0, 0);
}
else
if ((y_max - y_min) > X_SteadyThreshold)
{
SET_WARMUP_FLAG;
QueDelay (ClearWarmup, 1000);
sprintf (Buf, "Noise Y max:%x min:%x", y_max, y_min);
if (ProgramMode)
lcd_print (LEFT, 5, 1, Buf, 0, 0);
}
else
{
num_steady_samples--;
if (num_steady_samples == 0)
{
if (x_steady < 0)
x_steady -= STEADY_SAMPLES / 2;
else
x_steady += STEADY_SAMPLES / 2;
if (y_steady < 0)
y_steady -= STEADY_SAMPLES / 2;
else
y_steady += STEADY_SAMPLES / 2;
x_offset = x_steady / STEADY_SAMPLES;
y_offset = y_steady / STEADY_SAMPLES;
x_sum += STEADY_SAMPLES / 2;
y_sum += STEADY_SAMPLES / 2;
z_sum += STEADY_SAMPLES / 2;
x_sum /= STEADY_SAMPLES;
y_sum /= STEADY_SAMPLES;
z_sum /= STEADY_SAMPLES;
dax = x_sum / Gain_AX;
day = y_sum / Gain_AX;
daz = z_sum / Gain_AX;
grv_axyz_mag_ref = Root((dax*dax) + (day*day) + (daz*daz));
sign = (int)(grv_axyz_mag_ref * 10);
sprintf (Buf, "X=%x Y=%x GV=%d", x_offset, y_offset, sign);
if (ProgramMode)
lcd_print (LEFT, 6, 1, Buf, 0, 0);
LED_PORT &= ~GRN_LED;
Write_NVRAM();
LED_PORT |= GRN_LED;
CLR_STEADY_FLAG;
Gyro_Off();
}
}
}

//-------------------------------------------
//gyro_get_report
//-------------------------------------------
unsigned char gyro_get_report(GYRO_REPORT *report)
{
if(WARMUP_FLAG_TRUE)
{
CLR_DATA_FLAG;
return (0);
}
if (DATA_FLAG_TRUE)
{
report->x = Xdata;
report->y = Ydata;
if(Xdata || Ydata)
return (1);
return (0);
}
return (2);
}

void DoCalcs (void)
{
if (STEADY_FLAG_TRUE)
{
CLR_RAW_FLAG;
if(WARMUP_FLAG_TRUE)
Reset_Self_Steady();
else
Self_Steady();
return;
}
P3OUT |= 0x20;
switch (CalcState)
{
case 0:
CLR_RAW_FLAG;
SET_CALC_FLAG;
dax = -accel_x/Gain_AX;
day = -accel_y/Gain_AX;
daz = accel_z/Gain_AX;
CalcState++;
break;

case 1 :
// Our roll compensation dead zone.
gvisya = ((gvisya*19) + (Dabs(day)/Root(dax*dax+daz*daz)))*0.05;
CalcState++;
break;

case 2:
if(gvisya <= GVECTOR_ZERO)
//V^2 magnitude cutoff. GUL limits roll compensation by additive
velocity in one
//direction of mag(AX,AY,AZ) relitive to G-vector, while GLL limits roll
//compensation by subtractive velocity of the total accelermeter
magnitude.
grv_axyz_mag = Root((dax*dax) + (day*day) + (daz*daz));
CalcState++;
break;

case 3:
//thetal = ((Dabs(theta1 * 1 ) + Dabs(theta3))/ 2);//////////////////
//if (theta3 < 0)///////////////
// thetal = -Dabs(theta3);/////////////////////

if((grv_axyz_mag <= (grv_axyz_mag_ref)+0.75)&&(grv_axyz_mag >=
(grv_axyz_mag_ref-0.75)))
theta2 = Get_Theta(dax, daz);//
CalcState++;
break;

case 4:
//Non-Linear Delta Decimation:Uber Good
/*
if((theta1 - theta2) >= 0)
theta2 = ((Root(Dabs(theta1 - theta2)) + (theta2)));
else
theta2 = ((theta2) - (Root(Dabs(theta1 - theta2))));
*/
//theta2 = theta1;///////////////// no non linear filter
//Add Simple Backward Weighted Averaging: OK, Done at last.

theta3 = ((Dabs(theta3 * 9) + Dabs(theta2))*0.1);
if(theta2 < 0)
theta3 = -Dabs(theta2);


/////////////theta3 = -PI/2;//////////// for debuging Epsons.
//theta3 = theta2;///////// for debuging theta 2,3 Filters
CalcState++;
break;

case 5:
// Change Yaw and Pitch into magnitude and angle.
Calc_Mickeys();
rrx = x_mickeys;
rry = y_mickeys;
rr_mag = Root(rrx*rrx + rry*rry);
CalcState++;
break;

case 6:
theta4 = Get_Theta(x_mickeys, y_mickeys);
CalcState++;
//if((rr_mag) > (double)(Gain_X))/////////////// theta3 restore from
thetal filter cutoff
//{
//theta3 = thetal;//////////////////
//}
break;

case 7:
//Roll compensate Yaw and Pitch calculations.
rrx = rr_mag * Swine(theta4 + Pl - theta3);
CalcState++;
break;

case 8:
rry = rr_mag * Swine(theta4 + PIBY2 - theta3 );
x_mickeys = (signed int)(rrx + extra_yaw);
y_mickeys = (signed int)(rry + extra_pitch);
CalcState++;
break;

case 9:
// Residual from gain division.
extra_yaw = ((x_mickeys) % Gain_X);
extra_pitch = ((y mickeys) % Gain_Y);
// Stop accumulating residual offset.
// extra_yaw = (double)((int)(extra_yaw * 0.1 ) * 10);
// extra_pitch = (double)((int)(extra_pitch * 0.1 ) * 10);

if(SENT_FLAG_TRUE || WARMUP_FLAG_TRUE)
{
CLR_SENT_FLAG;
CLR_UPDATE_FLAG;
Xdata = 0;
Ydata = 0;
}
else
SET_UPDATE_FLAG;
if(jerk_timer)
{
Xdata = 0;
Ydata = 0;
}
else
{
Xdata += (signed char)(x_mickeys / Gain_X);
Ydata += (signed char)(-y_mickeys / Gain_Y);
}
CalcState = 0;
CLR_CALC_FLAG;
SET_DATA_FLAG;
break;

default:
Trap (2);
}
P3OUT &= -0x20;
}

//-------------------------------------------
// Dabs
//-------------------------------------------
double Dabs(double x)
{
if(x < 0)
x = -x;
return x;
}

//-------------------------------------------
// Swine
//-------------------------------------------
double Swine(double x)
{
double even, y;

even = (int)(x/(Pl));
x = x - (Pl*even);
if((int)(even/2)!= (even/2))
{
x = -x;
}
y = COEFF_3 * x + COEFF_4 * x * Dabs(x);
//y = COEFF_5 * (y * Dabs(y) - y) + y;
return y;
}

//-------------------------------------------
// Get_Theta
//-------------------------------------------
double Get_Theta(double x, double y)
{
double abs_y, r, angle;

abs_y = Dabs(y) + 0.0000000001F; // Prevent /0 condition

if (x >= 0)
{
r = (x - abs_y)/(x + abs_y);
angle = COEFF_1 - COEFF_1 * r;
}
else
{
r = (x + abs_y) / (abs_y - x);
angle = COEFF_2 - COEFF_1 * r;
}
if (y < 0)
retum(-angle); // negate if in quad III or IV
else
retum(angle);
}

//-------------------------------------------
// Root
//-------------------------------------------
double Root(double number)
{
long i;
float x, y, z;
const float f = 1.5F;
z = (float)number;
x = z*0.5F;
y =z;
i = * (long *) &y;
i = Ox5f3759df - ( i >> 1 );
y = * (float *) &i;
//y =y*(f-(x*y*y));
//y =y*(f-(x*y*y));
return (double)(z*y);
}

本発明は様々な改良を加えたり代替的形態にしても良いが、特定の実施形態は図面の例によって示されており、本明細書で詳細に説明されている。しかしながら、本発明は開示された特定の形態に限定される意図がないことが理解されるべきである。もしろ、本発明は、特許請求の範囲によって定義された本発明の趣旨及び範囲内にある全ての改良、等価のもの、及び代替物を網羅する。

Claims (21)

1. 制御装置（１１０）においてロール補償を提供する方法であって、
   前記制御装置（１１０）の動きを示す回転データ及び直線データを獲得するステップ（３１０）と、
   獲得したデータにロール補償を適用するステップ（３２０）と、
   ロール補償したデータからロール補償エラーを除去するステップ（３３０）と
   を備えることを特徴とする方法。
2. 前記回転データを獲得するステップ（３１０）は、ジャイロスコープセンサから回転データを受信するステップを含むことを特徴とする請求項１記載の方法。
3. 前記直線データを獲得するステップ（３１０）は、加速度計から直線データを受信するステップを含むことを特徴とする請求項１記載の方法。
4. 前記獲得したデータにロール補償を適用するステップ（３２０）は、獲得した前記回転データにロール補償を適用することを含むことを特徴とする請求項１記載の方法。
5. 前記ロール補償エラーを除去するステップ（３３０）は、前記ロール補償エラーがロール補償した前記回転データから除去されるように前記獲得した直線データを処理することを含むことを特徴とする請求項５記載の方法。
6. 前記ロール補償を適用するステップ（３２０）は、参照のフレームを変換することなく前記獲得したデータについてロール補償を適用することを含むことを特徴とする請求項１記載の方法。
7. 前記ロール補償エラーを除去するステップ（３３０）は、制御装置（１１０）の遠心加速度及び直線加速度を計算することなく前記ロール補償したデータからロール補償エラーを除去することを含むことを特徴とする請求項１記載の方法。
8. 前記ロール補償エラーが前記ロール補償したデータから除去された後、前記ロール補償したデータを用いてグラフィカル要素の移動を制御するステップを更に備えることを特徴とする請求項１記載の方法。
9. 前記グラフィカル要素はカーソル、ハイライトボックス、メニュー、ゲームキャラクタ及びグラフィカル対象物のうちの１つであることを特徴とする請求項８記載の方法。
10. 前記ロール補償エラーが前記ロール補償したデータから除去された後、前記ロール補償したデータを用いて移動可能な装置の移動を制御するステップを更に備えることを特徴とする請求項１記載の方法。
11. 前記移動可能な装置は、リモート制御される自動車、リモート制御される飛行機、リモート制御されるボート、及びリモート制御される電気機械装置のうちの１つであることを特徴とする請求項１０記載の方法。
12. 前記制御装置（１１０）は、リモート制御装置、マウス、ゲームコントローラ、仮想現実コントローラ、ポインタ、モバイル装置、及びディジタルメディア入力装置のうちの１つであることを特徴とする請求項１記載の方法。
13. 前記制御装置（１１０）は、ハンドヘルドプラットフォームに統合されていることを特徴とする請求項１記載の方法。
14. 前記ハンドヘルドプラットフォームは、ゲームコンソール、ＰＤＡ、携帯電話機、無線電話機、及びモバイルＴＶのうちの１つであることを特徴とする請求項１３記載の方法。
15. 制御装置（１１０）の動きを示す回転データ及び直線データを獲得する手段（２３０，２４０，２５０）と、
    獲得したデータにロール補償を適用する手段（２００）と、
    ロール補償したデータからロール補償エラーを除去する手段（２００）と
    を備えることを特徴とする制御装置（１１０）。
16. 前記回転データを獲得する手段は、ジャイロスコープセンサ（２３０，２４０）であることを特徴とする請求項１５記載の制御装置（１１０）。
17. 前記直線データを獲得する手段（２５０）は、加速度計を含むことを特徴とする請求項１５記載の制御装置（１１０）。
18. 前記獲得したデータに前記ロール補償を適用する手段（２００）は、当該獲得した回転データにロール補償を適用する手段を含むことを特徴とする請求項１５記載の制御装置（１１０）。
19. 前記ロール補償エラーを除去する手段（２００）は、前記ロール補償エラーが前記ロール補償した回転データから除去されるように、前記獲得した回転データを処理する手段を含むことを特徴とする請求項１８記載の制御装置（１１０）。
20. 前記ロール補償を適用する手段（２０００）は、参照のフレームを変換することなく、前記獲得したデータについてロール補償を適用する手段を含むことを特徴とする請求項１５記載の制御装置（１１０）。
21. 前記ロール補償エラーを除去する手段（２００）は、制御装置（１１０）の遠心加速度及び直線加速度を計算することなく、前記ロール補償したデータからロール補償エラーを除去する手段を含むことを特徴とする請求項１５記載の制御装置（１１０）。

Images