1 /*---------------------------------------------------------------------------* 2 Project: horizon 3 File: hid_GyroscopeReader.h 4 5 Copyright 2010 Nintendo. All rights reserved. 6 7 These coded instructions, statements, and computer programs contain 8 proprietary information of Nintendo of America Inc. and/or Nintendo 9 Company Ltd., and are protected by Federal copyright law. They may 10 not be disclosed to third parties or copied or duplicated in any form, 11 in whole or in part, without the prior written consent of Nintendo. 12 13 $Rev: $ 14 *---------------------------------------------------------------------------*/ 15 16 /*! @file 17 @brief GyroscopeReaderクラスを定義します 18 19 */ 20 21 #ifndef NN_HID_CTR_GYROSCOPEREADER_H_ 22 #define NN_HID_CTR_GYROSCOPEREADER_H_ 23 24 #include <nn/Handle.h> 25 #include <nn/Result.h> 26 #include <nn/types.h> 27 #include <nn/hid/CTR/hid_Result.h> 28 #include <nn/hid/CTR/hid_Api.h> 29 #include <nn/hid/CTR/hid_Gyroscope.h> 30 #include <nn/hid/CTR/hid_AccelerometerReader.h> 31 #include <nn/hid/CTR/hid_DeviceStatus.h> 32 #include <nn/util/util_SizedEnum.h> 33 #include <nn/util/util_NonCopyable.h> 34 35 namespace nn 36 { 37 namespace hid 38 { 39 namespace CTR 40 { 41 42 /*! 43 @brief ゼロ点のドリフト修正の強度を設定する列挙体です 44 */ 45 enum ZeroDriftMode 46 { 47 GYROSCOPE_ZERODRIFT_LOOSE, //!< センサの値がある程度変動していても、安定しているとみなして修正が行われるモードです。 48 GYROSCOPE_ZERODRIFT_STANDARD,//!< GYROSCOPE_ZERODRIFT_LOOSE と GYROSCOPE_ZERODRIFT_TIGHT の中間にあたるモードです。 49 GYROSCOPE_ZERODRIFT_TIGHT,//!< センサの値が定められた閾値以内の変動でないと、安定しているとみなさない厳密な修正が行われるモードです。 50 GYROSCOPE_ZERODRIFT_NUM 51 }; 52 53 54 /*! 55 @brief ジャイロセンサのサンプリングデータを読み込むクラスです。 56 */ 57 class GyroscopeReader: private nn::util::NonCopyable<GyroscopeReader> 58 { 59 public: 60 61 62 /*! 63 @name コンストラクタ/デストラクタ 64 @{ 65 */ 66 67 /*! 68 @brief コンストラクタです。 69 70 インスタンスを生成する前に @ref nn::hid::CTR::Initialize( ) で初期化してください。<BR> 71 サンプリングを開始するために必要な設定を行います。<BR> 72 最初のインスタンスを生成してからサンプリングが開始されるまでにウェイト時間が必要な点にご注意ください。 73 @param[in] pAccelerometrReader AccelerometerReaderへの参照を指定すると、参照するAccelerometerReaderの出力値を利用した補正を行います。 74 デフォルトではNULLが渡され、この場合内部に保持するデフォルトのAccelerometerReaderによって補正を行います。 75 76 */ 77 GyroscopeReader(AccelerometerReader* pAccelerometerReader = NULL, Gyroscope& gyroscope = GetGyroscope()); 78 79 /*! 80 @brief デストラクタです。 81 */ 82 83 ~GyroscopeReader(); 84 85 /*! 86 @} 87 */ 88 89 /*! 90 @name サンプリングデータの読み込み 91 @{ 92 */ 93 94 /*! 95 @brief ジャイロセンサのサンプリングデータを新しいものから順に読み込みます。以前に読み込んだデータは読み込まれません。 96 97 @param[out] pBufs 新しいものから順にサンプリングデータが読み込まれます。 98 @param[out] pReadLen 読み込んだサンプリングデータの数です。 99 @param[in] bufLen pBufs に読み込めるサンプリングデータの数を指定します。 100 @return なし。 101 102 */ 103 void Read(GyroscopeStatus* pBufs, s32* pReadLen, s32 bufLen); 104 105 /*! 106 @brief 最新のジャイロセンサのサンプリングデータを読み込みます。 nn::hid::CTR::GyroscopeReader::Read( ) とちがい同じサンプリングデータを読み込むことが出来ます。 107 108 @param[out] pBuf サンプリングデータが読み込まれます。 109 @return サンプリングデータの読み込み結果を返します。<BR> 110 true ・・・ 読み込めました。<BR> 111 false ・・・ 読み込めませんでした。(GyroscopeReaderのインスタンス生成後あるいはスリープ復帰後の最初のサンプリングが行われ次第、読み込めます。) 112 113 */ 114 bool ReadLatest(GyroscopeStatus* pBuf); 115 116 /*! 117 @} 118 */ 119 120 /*! 121 @name リセット 122 @{ 123 */ 124 125 /*! 126 @brief 内部状態を初期化します。 127 128 このとき、各種補正処理の設定も以下のようになります。 129 130 加速度の補正 有効になります。<BR> 131 ゼロ点のドリフト補正 GYROSCOPE_ZERODRIFT_STANDARD に設定され、有効になります。<BR> 132 ゼロ点の遊び 無効になります。<BR> 133 軸回転 回転行列に単位行列が設定され、さらに無効になります。<BR> 134 角速度の倍率 すべて1.0 に設定されます<BR> 135 136 @return なし。 137 */ 138 139 void Reset(); 140 141 /*! 142 @} 143 */ 144 145 /*! 146 @name 出力値の設定 147 @{ 148 */ 149 150 151 /*! 152 @brief Read 関数で得られる GyroscopeStatus 構造体の angle を任意の値にリセットします。 153 @param[in] ax ピッチ方向の角度。角度は 1.0 を 360度とした値です。 154 @param[in] ay ヨー方向の角度。角度は 1.0 を 360度とした値です。 155 @param[in] az ロール方向の角度。角度は 1.0 を 360度とした値です。 156 @return なし。 157 */ 158 void SetAngle(f32 ax, f32 ay, f32 az); 159 160 /*! 161 @brief Read 関数で得られる GyroscopeStatus 構造体の direction を任意の値にリセットします。 162 @param[in] direction 設定する任意の3次元姿勢。 163 @return なし。 164 */ 165 void SetDirection(const Direction& direction); 166 167 /*! 168 @brief 角速度を計算する際に掛ける倍率を各方向ごとに設定します。 169 @param[in] pitch pitch方向の角速度を計算する際に掛ける倍率。 170 @param[in] yaw yaw方向の角速度を計算する際に掛ける倍率。 171 @param[in] roll roll方向の角速度を計算する際に掛ける倍率。 172 @return なし。 173 */ 174 void SetAngleMagnification(f32 pitch, f32 yaw, f32 roll); 175 176 /*! 177 @brief Read 関数で得られる GyroscopeStatus 構造体の direction を内部で計算する際に使用する角速度の値にかける倍率を設定します。 178 179 たとえば、magification を 2.0f に設定した場合、90度回転させると GyroscopeStatus 構造体の direction は180度回転した姿勢として計算されます。 180 181 @param[in] magnification 三次元姿勢を計算する際に使用する角速度にかける倍率。 182 @return なし。 183 */ 184 void SetDirectionMagnification(f32 magnification); 185 186 /*! 187 @} 188 */ 189 190 /*! 191 @name ゼロ点の遊び 192 @{ 193 */ 194 195 /*! 196 @brief ゼロ点の遊びを有効にします。 197 @return なし。 198 */ 199 void EnableZeroPlay(); 200 201 /*! 202 @brief ゼロ点の遊びを無効にします。 203 @return なし。 204 */ 205 void DisableZeroPlay(); 206 207 /*! 208 @brief ゼロ点の遊びが有効になっているか調べます。 209 @return 有効な場合にtrue,無効な場合にfalseが帰ります。 210 */ 211 bool IsEnableZeroPlay() const; 212 213 /*! 214 @brief ゼロ点の遊びによる補正処理の働き具合を返します。 215 216 ゼロ点の遊びが無効の場合は0を返します。 217 218 @return 無効の場合は常に0が返ります。有効の場合は0以上の値が返ります。この値は補正処理の働き具合を示します。 219 220 現在の角速度が設定した遊びの値に近いほど0に近づき、遊びの範囲を越える速度になると0になります。 221 */ 222 f32 GetZeroPlayEffect() const; 223 224 225 /*! 226 @brief 静止時（ゼロ点）の遊びを設定します。 227 228 遊びを設けると小さな動きに反応しなくなります。 229 230 設定した遊びはすべての方向に対して適用されます。 231 232 @param[in] radius ゼロ点の遊び具合。1.0f を 360 dps とした値を設定します。 233 @return なし。 234 */ 235 void SetZeroPlayParam(f32 radius); 236 237 /*! 238 @brief ゼロ点の遊びを取得します。 239 240 遊びは 1.0f を 360 dps とした値になります。 241 242 @param[out] radius 現在のゼロ点の遊び具合をコピーするバッファ。 243 @return なし。 244 */ 245 void GetZeroPlayParam(f32& radius) const; 246 247 /*! 248 @brief ゼロ点の遊びを初期化します。 249 250 初期値は 0.005f となります。 251 252 @return なし。 253 */ 254 void ResetZeroPlayParam(); 255 256 /*! 257 @} 258 */ 259 260 261 /*! 262 @name ゼロ点ドリフトの補正 263 @{ 264 */ 265 266 /*! 267 @brief ゼロ点ドリフトの補正を有効にします。 268 @return なし。 269 */ 270 void EnableZeroDrift(); 271 272 /*! 273 @brief ゼロ点ドリフトの補正を無効にします。 274 @return なし。 275 */ 276 void DisableZeroDrift(); 277 278 /*! 279 @brief ゼロ点ドリフトの補正が有効になっているか調べます。 280 281 ゼロ点ドリフトの補正は初期状態では有効となっています。 282 283 @return 有効な場合にtrue,無効な場合にfalseを返します。 284 */ 285 bool IsEnableZeroDrift() const; 286 287 /*! 288 @brief ゼロ点ドリフトの補正の効き具合を取得します。 289 290 この値は補正処理の働き具合を示し、１に近づくほどそのときの角速度で安定していることを意味します。 291 292 @return 無効の場合は負の値が返ります。有効の場合はゼロ以上の値が返ります。この値は補正処理の働き具合を示します。 293 294 １に近づくほどそのときの角速度で安定していることを意味します。 295 296 また、ゼロが返される期間は補正処理は行われません。 297 */ 298 f32 GetZeroDriftEffect() const; 299 300 /*! 301 @brief ゼロ点ドリフトの補正モードを初期化します。 302 303 初期値は GYROSCOPE_ZERODRIFT_STANDARD に設定されます。 304 305 @return なし。 306 */ 307 void ResetZeroDriftMode(); 308 309 /*! 310 @brief ゼロ点ドリフトの補正モードを設定します。 311 312 設定した内容はすべての方向に対して適用されます。 313 314 GYROSCOPE_ZERODRIFT_LOOSE の場合、センサーの値がある程度変動していても安定しているとみなし 315 ゼロ点の値を修正しようとしますので、等速度運動時にも修正される場合があります。 316 317 そのような場合はより条件が厳しいモードに設定することで改善される場合があります。 318 319 デフォルトは GYROSCOPE_ZERODRIFT_STANDARD に設定されています。 320 321 @param[in] mode ゼロ点ドリフトの補正モード 322 @return なし。 323 */ 324 void SetZeroDriftMode(const ZeroDriftMode& mode); 325 326 /*! 327 @brief ゼロ点ドリフトの補正モードを取得します。 328 @param[out] mode 現在のゼロ点ドリフトの補正モードをコピーするバッファ。 329 @return なし。 330 */ 331 void GetZeroDriftMode(ZeroDriftMode& mode) const; 332 333 /*! 334 @} 335 */ 336 337 338 /*! 339 @name 加速度補正 340 @{ 341 */ 342 343 /*! 344 @brief 加速度補正を有効にします。 345 346 加速度補正はCTRの静止時に加速度センサの値を用いてジャイロセンサの姿勢計算を補正する機能です。 347 348 加速度補正が有効であるとき、ジャイロセンサと補正に使用する加速度センサの軸回転に関する設定が共通である必要があります。 349 350 詳細は nn::hid::GyroscopeReader::EnableAxisRotation( ) を参照してください。 351 352 @return なし。 353 */ 354 void EnableAccRevise(); 355 356 /*! 357 @brief 加速度補正を無効にします。 358 @return なし。 359 */ 360 void DisableAccRevise(); 361 362 /*! 363 @brief 加速度補正が有効になっているか調べます。 364 365 加速度補正は初期状態では有効となっています。 366 367 @return 有効な場合にtrue,無効な場合にfalseを返します。 368 */ 369 bool IsEnableAccRevise() const; 370 371 /*! 372 @brief 加速度補正処理の働き具合が返されます。 373 @return 加速度補正処理が無効の場合は常に0を返します。有効の場合は0以上の値が返ります。 374 375 この値は補正処理の働き具合を示し、１に近いほどRead関数・ReadLatest関数で得られるGyroscopeStatus.directionの方向を加速度の方向に近づけるように補正がかかっています。 376 */ 377 f32 GetAccReviseEffect() const; 378 379 380 /*! 381 @brief 加速度補正の重みと有効範囲を設定します。 382 383 設定した内容はすべての方向に対して適用されます。 384 385 @param[in] revisePower 加速度補正の重み。0 から 1 の間の値を設定してください。値が大きいほど急激に補正がかかります。 386 @param[in] reviseRange 加速度補正の有効範囲。重力加速度を中心にこの範囲内の加速度センサの値を補正計算に使用します。 387 388 1.0f は重力加速度の大きさを意味します。 389 390 つまり、0.4f を指定した場合は 0.6f から 1.4f の範囲に収まる加速度センサの値を補正計算に使用します。 391 @return なし。 392 */ 393 void SetAccReviseParam(f32 revisePower, f32 reviseRange); 394 395 /*! 396 @brief 加速度補正の重みと有効範囲を取得します。 397 @param[out] revisePower 現在の加速度補正具合をコピーするバッファ。 398 @param[out] reviseRange 現在の加速度補正の有効半径をコピーするバッファ。 399 @return なし。 400 */ 401 void GetAccReviseParam(f32& revisePower, f32& reviseRange) const; 402 403 /*! 404 @brief 加速度補正の重みと有効範囲を初期化します。 405 406 初期値は加速度補正処理の重み(revisePower)が 0.030f、加速度補正の有効半径(reviseRange)が 0.400f となります。 407 408 @return なし。 409 */ 410 void ResetAccReviseParam(); 411 412 /*! 413 @} 414 */ 415 416 /*! 417 @name 軸回転 418 @{ 419 */ 420 421 /*! 422 @brief 軸回転を有効にします。 423 424 軸回転が有効なとき、nn::hid::CTR::GyroscopeReader::SetRotationAxis( ) で設定された回転行列使用し、サンプリングデータに回転変換処理を行った値が出力されるようになります。 425 426 この処理によって、CTR内にセンサが傾いて配置されているかのように扱うことができます。 427 428 軸回転を行い、かつ加速度補正も行う場合には、加速度センサの設定に注意する必要があります。<BR> 429 この場合には、参照しているAccelerometerReaderの軸回転とジャイロセンサの軸回転の設定が共通でなければいけません。<BR> 430 デフォルトのAccelerometerReaderを使用している場合には nn::hid::CTR::GyroscopeReader::EnableAxisRotation( ), nn::hid::CTR::GyroscopeReader::DisableAxisRotation( ), 431 nn::hid::CTR::GyroscopeReader::SetAxisRotationMatrix( ), nn::hid::CTR::GyroscopeReader::ResetAxisRotationMatrix( ) の各関数コール時にSDK側でデフォルトの 432 @ref nn::hid::CTR::AccelerometerReaderの対応する関数をコールして設定を共通化しますが、コンストラクタで独自の@ref nn::hid::CTR::AccelerometerReaderインスタンスを 433 指定した場合には、これらの設定をアプリ側で揃える必要があります。<BR> 434 軸回転の設定が共通でない場合、加速度補正が不適切に働く可能性がありますのでご注意ください。<BR> 435 436 本設定は初期値では無効になっています。 437 */ 438 void EnableAxisRotation(); 439 440 /*! 441 @brief 軸回転を無効にします。 442 443 本設定は初期値では無効になっています。 444 */ 445 void DisableAxisRotation(); 446 447 /*! 448 @brief 軸回転が有効か無効か調べます。 449 @return 軸回転が有効であればtrue,無効であればfalseを返します。 450 */ 451 bool IsEnableAxisRotation(); 452 453 /*! 454 @brief 軸回転で使用する回転行列を設定します。 455 */ 456 void SetAxisRotationMatrix(nn::math::MTX34 mtx); 457 458 /*! 459 @brief 軸回転で使用する回転行列を取得します。 460 461 @return 設定されている回転行列 462 */ 463 nn::math::MTX34 GetAxisRotationMatrix(); 464 465 /*! 466 @brief 軸回転で使用する回転行列を初期化します。 467 この時回転行列は nn::math::MTX34 の単位行列に設定され、これは軸回転が無効な場合と同等です。 468 */ 469 void ResetAxisRotationMatrix(); 470 471 /*! 472 @} 473 */ 474 475 private : 476 477 void ReadLocal(GyroscopeStatus* pBufs, s32* pReadLen, s32 bufLen); 478 void GetZeroDriftParam(f32& radius, s32& count, f32& power) const; 479 void SetZeroDriftParam(f32 radius, s32 count, f32 power); 480 f32 ReviseDirection_Acceleration(Direction& rev_dir, const nn::math::VEC3& acc); 481 void CalculateGyroscopeAxisStatus( 482 f32 *destSpeed, 483 s32 *nearSamplingNum, 484 f32 *zeroOffset, 485 s32 srcSpeed, 486 f32 speedScale, 487 s32* oldValueArray); 488 void CalculateDirection(); 489 void InitializeCalibrationData(); 490 491 private : 492 /* ドリフト修正カウントの最大数*/ 493 static const u32 GYROSCOPE_DRIFT_COUNT_MAX = 256; //2のn乗であること 494 495 496 private : 497 static const s32 GYROSCOPE_LOCAL_BUFFER_SIZE = 32; 498 s32 m_GyroscopeLocalBufferSize; 499 GyroscopeStatus m_GyroscopeStatusLocalBuffer[GYROSCOPE_LOCAL_BUFFER_SIZE]; 500 bool m_IsFirstRead; 501 NN_PADDING3; 502 503 AccelerometerReader m_DefaultAccelerometerReader; 504 505 GyroscopeStatus m_CurrentStatus; 506 AccelerometerReader* m_pAccelerometerReader; 507 508 nn::math::VEC3 m_SpeedOld; 509 nn::math::VEC3 m_SpeedVector; 510 nn::math::VEC3 m_SpeedScale; 511 f32 m_DirectionMagnification; // 姿勢計算時の倍率 512 513 f32 m_Period; // 計算用係数 514 f32 m_FreqDegree; // 計算用係数 515 f32 m_FreqRadian; // 計算用係数 516 517 bool m_EnableZeroPlay; // ゼロ点に遊びを設けるか 518 bool m_EnableZeroDrift; // ゼロ点自動補正するか 519 bool m_EnableAccRevise; // 加速度で姿勢補正するか 520 bool m_EnableRotate; 521 522 f32 m_ZeroPlayRadius; // ゼロ点遊び半径 (1 = 360deg/sec) 523 f32 m_ZeroDriftRadius; // ドリフトカウント許容半径 (1 = 360deg/sec) 524 s32 m_ZeroDriftCount; // ドリフト修正カウント 525 f32 m_ZeroDriftPower; // ドリフト修正パワー 526 f32 m_AccRevisePower; // 加速度補正力 527 f32 m_AccReviseRange; // 加速度補正有効半径 528 529 f32 m_ZeroPlayEffect; // 遊び円の内側具合 0:外側 ～ 1:中心 530 f32 m_ZeroDriftEffect; // ドリフト修正カウントの続き具合 (0～1) 531 f32 m_AccRevEffect; // 加速度補正がどれくらいかかっているか 532 533 nn::math::VEC3 m_CalibrationZero; // キャリブレーション値 534 f64 m_CalibrationScale[3]; // キャリブレーション値 535 536 nn::math::VEC3 m_CountZero; // ゼロ点を示すカウント値 537 s32 m_CountIdx; // 最新が格納されているバッファインデクス 538 s32 m_CountT[3][GYROSCOPE_DRIFT_COUNT_MAX]; // XYZハードウェア値 539 f32 m_DpsPitchMagnification; // pitch方向の角速度計算時の倍率 540 f32 m_DpsYawMagnification; // yaw方向の角速度計算時の倍率 541 f32 m_DpsRollMagnification; // roll方向の角速度計算時の倍率 542 NN_PADDING4; 543 544 /*from Low*/ 545 Gyroscope& m_Gyroscope; 546 s32 m_IndexOfRead; 547 s64 m_TickOfRead; 548 549 /*rotate axis*/ 550 nn::math::MTX34 m_RotateMtx; 551 552 }; 553 554 } // namespace CTR { 555 } // namespace hid { 556 } // namespace nn { 557 558 #endif // #ifndef NN_HID_CTR_GYROSCOPEREADER_H_ 559