Ryota Yamamoto
BNO055をI2CとUARTで使用するためのライブラリ。UARTはmbedにて不安定なため使用できるボードとできないボードがある模様。
Dependents: BNO055_BME280_ Yabusame2_gyro GRhanawaizman test_deg_read
BNO055.cpp
- Committer:
- Yajirushi
- Date:
- 2017-06-25
- Revision:
- 3:363ec3772dce
- Parent:
- 1:7dbc931127c0
- Child:
- 4:9efb4abafd2e
File content as of revision 3:363ec3772dce:
#include "BNO055.h"
//CLASS:BNO055_CTRL//////////////////////////////////////////////////
/* ------------------------------------------------------------------
* BNO055_UART_CTRLクラスとBNO055_I2C_CTRLクラスの基底クラス(インターフェース)
* UARTとI2Cをヘッダ書き換えなしで実現するために無理するとこうなった
*/
/* ==================================================================
* デフォルトコンストラクタ
*/
BNO055_CTRL::BNO055_CTRL(){
lastError = 0;
lastLength = 0;
}
/* ==================================================================
* デフォルトデストラクタ
*/
BNO055_CTRL::~BNO055_CTRL(){}
/* ==================================================================
* 現在のページIDを取得する
*/
char BNO055_CTRL::getNowPage(){
return page1 ? 1 : 0;
}
/* ==================================================================
* UARTまたはI2Cで取得した最後のエラーを取得する：通信がうまくいかないときの原因追及
*/
char BNO055_CTRL::getLastError(){
return lastError;
}
/* ==================================================================
* UARTまたはI2Cで通信した際の受信バイト数を取得する：通信がうまくいかないときの原因追及
*/
char BNO055_CTRL::getLastLength(){
return lastLength;
}
/* ==================================================================
* 未実装関数(子クラスで実装される)
*/
void BNO055_CTRL::init(){}
char BNO055_CTRL::rr(bool isPage1, char regAddr){return 0;}
char BNO055_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){return 0;}
char BNO055_CTRL::wr(bool isPage1, char regAddr, char wBytes){return 0;}
char BNO055_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){return 0;}
//CLASS:BNO055_UART_CTRL/////////////////////////////////////////////
/* ------------------------------------------------------------------
* BNO055_CTRLクラス(インターフェース)を継承(実装)したクラス
* UARTで命令を送受信するためのコントロール用クラス
*/
/* ==================================================================
* BNO055をUARTでコントロールするためのクラス：コンストラクタ
*/
BNO055_UART_CTRL::BNO055_UART_CTRL(RawSerial *uart){
iface = uart;
rxd = 0xFFFF;
read_mark = true;
page1 = true;
ary = new char[BNO055_UART_BUF_MAXLEN + 5];
memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5);
lastError = 0;
}
/* ==================================================================
* BNO055をUARTでコントロールするためのクラス：デストラクタ
*/
BNO055_UART_CTRL::~BNO055_UART_CTRL(){
delete iface;
}
/* ==================================================================
* UART受信割り込み用関数
* RX受信トリガがONになると、カウンタを加算する
*/
void BNO055_UART_CTRL::rxInterrupt(){
if(read_mark){
rxd = iface->getc();
read_mark = false;
}
}
/* ==================================================================
* <UART>
* レジスタの内容を読み取り(1byteのみ)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* それ以外 成功した際に取得したデータ
*/
char BNO055_UART_CTRL::rr(bool isPage1, char regAddr){
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0);
page1 = isPage1;
}
//送信可能になるまで待つ
do{wait_ms(1);}while(!iface->writeable());
//コマンドをセット
ary[0] = 0xAA; //StartByte(固定)
ary[1] = 0x01; //読み取り
ary[2] = regAddr; //レジスタアドレス
ary[3] = 1; //バイト長
//送信
iface->putc(ary[0]);
iface->putc(ary[1]);
iface->putc(ary[2]);
iface->putc(ary[3]);
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
lastError = 0;
memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア
ary[0] = rxd & 0xFF;
//残りを受信
int i = 1;
int cnt = (ary[0] == 0xBB) ? 2 : 1;
while(i < cnt && iface->readable()){
ary[i++] = iface->getc();
}
//レスポンスが0xBB以外:通信失敗
if(ary[0] != 0xBB){
lastLength = 2;
lastError = ary[1];
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return -1;
}
lastLength = ary[1] + 2;
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
//通信成功時、取得データを返す
return ary[2];
}
/* ==================================================================
* <UART>
* レジスタの内容を読み取り(複数可)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* -2 返答バイト不一致
* -4 レスポンスエラー
* それ以外 成功した際の戻り値バイト数
*/
char BNO055_UART_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){
//読み取りバイト数が1未満またはBNO055_UART_BUF_MAXLEN以上はバッファが足りないので読み取れない
if(length < 1 || length > BNO055_UART_BUF_MAXLEN) return -1;
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0);
page1 = isPage1;
}
//送信可能になるまで待つ
do{wait_ms(1);}while(!iface->writeable());
//コマンドをセット
ary[0] = 0xAA; //StartByte(固定)
ary[1] = 0x01; //読み取り
ary[2] = startRegAddr; //読み取り開始レジスタアドレス
ary[3] = length; //バイト長
//送信
iface->putc(ary[0]);
iface->putc(ary[1]);
iface->putc(ary[2]);
iface->putc(ary[3]);
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
lastError = 0;
memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア
ary[0] = rxd & 0xFF;
//残りを受信
int i = 1;
int cnt = (ary[0] == 0xBB) ? 2 : 1;
while(i < cnt && iface->readable()){
ary[i++] = iface->getc();
}
//レスポンスが0xBB以外:通信失敗
if(ary[0] != 0xBB){
lastLength = 2;
lastError = ary[1];
rxd = 0xFFFF;
read_mark = true;
return -1;
}
//返答バイト長がlengthと一致しない:通信失敗
if(ary[1] != length){
lastLength = ary[1];
lastError = 0;
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return -2;
}
lastLength = ary[1] + 2;
memcpy(receiveBytes, ary+2, ary[1]);
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return ary[1];
}
/* ==================================================================
* <UART>
* レジスタ書き込み(1byteのみ)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BNO055_UART_CTRL::wr(bool isPage1, char regAddr, char wBytes){
//ページが異なるならページ変更命令を発行(再帰処理)
if(page1 != isPage1){
wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0);
page1 = isPage1;
}
//送信可能になるまで待つ
do{wait_ms(1);}while(!iface->writeable());
//コマンドをセット
ary[0] = 0xAA; //StartByte(固定)
ary[1] = 0x00; //書き込み
ary[2] = regAddr; //レジスタアドレス
ary[3] = 1; //バイト長
ary[4] = wBytes; //送信データ
//送信
iface->putc(ary[0]);
iface->putc(ary[1]);
iface->putc(ary[2]);
iface->putc(ary[3]);
iface->putc(ary[4]);
//システムリブートが発生するレジスタの場合は1200ms待つ
if(regAddr == 0x3F) wait_ms(1200);
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
lastError = 0;
memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア
ary[0] = rxd & 0xFF;
//残りを受信
while(iface->readable()){
ary[1] = iface->getc();
}
//レスポンスが0xEE以外もしくはステータスが0x01以外:書き込み失敗
if(ary[0] != 0xEE || ary[1] != 0x01){
lastLength = 2;
lastError = ary[1];
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return -1;
}
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return 1;
}
/* ==================================================================
* <UART>
* レジスタ書き込み(複数可)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* -4 レスポンスエラー
* 1 成功
*/
char BNO055_UART_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){
//書き込みバイト数が1未満またはBNO055_UART_BUF_MAXLEN以上はバッファが足りないので読み取れない
if(length < 1 || length > BNO055_UART_BUF_MAXLEN) return -1;
//ページが異なるならページ変更命令を発行(再帰処理)
if(page1 != isPage1){
wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0);
page1 = isPage1;
}
//送信可能になるまで待つ
do{wait_ms(1);}while(!iface->writeable());
//コマンドをセット
ary[0] = 0xAA; //StartByte(固定)
ary[1] = 0x00; //書き込み
ary[2] = startRegAddr; //レジスタアドレス
ary[3] = length; //バイト長
//前部分送信
iface->putc(ary[0]);
iface->putc(ary[1]);
iface->putc(ary[2]);
iface->putc(ary[3]);
//データ内容送信
for(int cnt=0; cnt<length; cnt++){
iface->putc(Bytes[cnt]);
}
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
lastError = 0;
memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア
ary[0] = rxd & 0xFF;
//残りを受信
while(iface->readable()){
ary[1] = iface->getc();
}
//レスポンスが0xEE以外もしくはステータスが0x01以外:書き込み失敗
if(ary[0] != 0xEE || ary[1] != 0x01){
lastLength = 2;
lastError = ary[1];
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return -1;
}
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
return 1;
}
/* ==================================================================
* <UART>
* インターフェース設定を初期化する
*/
void BNO055_UART_CTRL::init(){
iface->format(); //8N1 = default
iface->baud(115200);
wait_ms(5);
iface->attach(this, &BNO055_UART_CTRL::rxInterrupt);
//送信可能になるまで待つ
do{wait_ms(1);}while(!iface->writeable());
//UARTリセット時に0xFFがつく現象および前回送信時の残りがあるため同期がとれない
//複数回読み取りを行い、正しいレスポンス(0xBB)が返るまで送信
iface->putc(0x01); //dummy
iface->putc(0x01); //dummy
iface->putc(0x01); //dummy
iface->putc(0x01); //dummy
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
char rslt = rxd & 0xFF; //レスポンス
do{
//レスポンスが異常:残りの情報を無視
if(rslt != 0xBB){
while(iface->readable()){
iface->getc();
}
}
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
//送信可能になるまで待つ
do{wait_ms(5);}while(!iface->writeable());
iface->putc(0xAA); //START
iface->putc(0x01); //読み取り
iface->putc(0x07); //ページID
iface->putc(0x01); //length 1byte
//レスポンスを受信するまで待つ
while(read_mark) wait_us(100);
rslt = rxd & 0xFF; //レスポンス
}while(rslt != 0xBB);
//残りの情報を無視
while(iface->readable()){
iface->getc();
}
//受信用割り込みマークをリセット
rxd = 0xFFFF;
read_mark = true;
}
//CLASS:BNO055_I2C_CTRL//////////////////////////////////////////////
/* ------------------------------------------------------------------
* BNO055_CTRLクラス(インターフェース)を継承(実装)したクラス
* I2Cで命令を送受信するためのコントロール用クラス
*/
/* ==================================================================
* BNO055をI2Cでコントロールするためのクラス：コンストラクタ
*/
BNO055_I2C_CTRL::BNO055_I2C_CTRL(I2C* iic, char addr, unsigned int freq){
iface = iic;
i2c_writeAddr = addr << 1;
i2c_readAddr = i2c_writeAddr + 1;
i2c_freq = freq;
page1 = true;
ary = new char[2];
memset(ary, 0, 2);
lastError = 0;
}
/* ==================================================================
* BNO055をI2Cでコントロールするためのクラス：デストラクタ
*/
BNO055_I2C_CTRL::~BNO055_I2C_CTRL(){
delete iface;
delete[] ary;
}
/* ==================================================================
* <I2C>
* レジスタの内容を読み取り(1byteのみ)
*/
char BNO055_I2C_CTRL::rr(bool isPage1, char regAddr){
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
ary[0] = BNO055_PAGE_ID;
ary[1] = (isPage1) ? 1 : 0;
page1 = isPage1;
iface->write(i2c_writeAddr, ary, 2);
}
ary[0] = regAddr;
iface->write(i2c_writeAddr, ary, 1, true);
iface->read(i2c_readAddr, ary, 1, false);
return ary[0];
}
/* ==================================================================
* <I2C>
* レジスタの内容を読み取り(複数可)
*/
char BNO055_I2C_CTRL::rrc(bool isPage1, char startRegAddr, unsigned char *receiveBytes, char length){
//読み取りバイト数が1未満
if(length < 1) return -1;
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
ary[0] = BNO055_PAGE_ID;
ary[1] = (isPage1) ? 1 : 0;
page1 = isPage1;
iface->write(i2c_writeAddr, ary, 2);
}
ary[0] = startRegAddr;
iface->write(i2c_writeAddr, ary, 1, true);
iface->read(i2c_readAddr, (char *)receiveBytes, length, false);
return receiveBytes[0];
}
/* ==================================================================
* <I2C>
* レジスタ書き込み(1byteのみ)
*/
char BNO055_I2C_CTRL::wr(bool isPage1, char regAddr, char wBytes){
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
ary[0] = BNO055_PAGE_ID;
ary[1] = (isPage1) ? 1 : 0;
page1 = isPage1;
iface->write(i2c_writeAddr, ary, 2);
}
ary[0] = regAddr;
ary[1] = wBytes;
iface->write(i2c_writeAddr, ary, 2);
return ary[0];
}
/* ==================================================================
* <I2C>
* レジスタ書き込み(複数可)
*/
char BNO055_I2C_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){
//書き込みバイト数が1未満
if(length < 1) return -1;
//ページが異なるならページ変更命令を発行
if(page1 != isPage1){
ary[0] = BNO055_PAGE_ID;
ary[1] = (isPage1) ? 1 : 0;
page1 = isPage1;
iface->write(i2c_writeAddr, ary, 2);
}
ary[0] = startRegAddr;
iface->write(i2c_writeAddr, ary, 1, true);
iface->write(i2c_writeAddr, Bytes, length, false);
return Bytes[0];
}
/* ==================================================================
* <I2C>
* インターフェース設定を初期化する
*/
void BNO055_I2C_CTRL::init(){
iface->frequency(i2c_freq);
wait_ms(5);
}
//CLASS:BOARDC_BNO055////////////////////////////////////////////////
/* ------------------------------------------------------------------
* メインクラス
* BNO055をUARTまたはI2Cで使用するための命令をまとめている
* 内部にBNO055_CTRLクラスのインスタンスを持ち、コンストラクタの引数によって
* UARTとI2Cのどちらを使用するか決定する
*/
/* ==================================================================
* コンストラクタ (オーバーロード+3)
* UARTで使用する際のコンストラクタ：ピン別名を指定する
*/
BOARDC_BNO055::BOARDC_BNO055(PinName tx, PinName rx){
ctrl = new BNO055_UART_CTRL(new RawSerial(tx, rx));
scaleACC = 0.01f; // = 1 / 100
scaleMAG = 0.0625f; // = 1 / 16
scaleGYRO = 0.0625f; // = 1 / 16
scaleEuler = 0.0625f; // = 1 / 16
scaleTEMP = 1.0f;
scaleLIA = scaleACC;
scaleGV = scaleACC;
scaleQuaternion = 0.00006103515; // = 1 / (2^14)
clkExt = false;
}
/* ==================================================================
* コンストラクタ (オーバーロード+3)
* UARTで使用する際のコンストラクタ：RawSerialクラスのインスタンスを指定する
*/
BOARDC_BNO055::BOARDC_BNO055(RawSerial *uart){
ctrl = new BNO055_UART_CTRL(uart);
scaleACC = 0.01f; // = 1 / 100
scaleMAG = 0.0625f; // = 1 / 16
scaleGYRO = 0.0625f; // = 1 / 16
scaleEuler = 0.0625f; // = 1 / 16
scaleTEMP = 1.0f;
scaleLIA = scaleACC;
scaleGV = scaleACC;
scaleQuaternion = 0.00006103515; // = 1 / (2^14)
clkExt = false;
}
/* ==================================================================
* コンストラクタ (オーバーロード+3)
* I2Cで使用する際のコンストラクタ：ピン別名を指定する
*/
BOARDC_BNO055::BOARDC_BNO055(PinName sda, PinName scl, char addr, unsigned int freq){
ctrl = new BNO055_I2C_CTRL(new I2C(sda, scl), addr, freq);
scaleACC = 0.01f; // = 1 / 100
scaleMAG = 0.0625f; // = 1 / 16
scaleGYRO = 0.0625f; // = 1 / 16
scaleEuler = 0.0625f; // = 1 / 16
scaleTEMP = 1.0f;
scaleLIA = scaleACC;
scaleGV = scaleACC;
scaleQuaternion = 0.00006103515; // = 1 / (2^14)
clkExt = false;
}
/* ==================================================================
* コンストラクタ (オーバーロード+3)
* I2Cで使用する際のコンストラクタ：I2Cクラスのインスタンスを指定する
*/
BOARDC_BNO055::BOARDC_BNO055(I2C* iic, char addr, unsigned int freq){
ctrl = new BNO055_I2C_CTRL(iic, addr, freq);
scaleACC = 0.01f; // = 1 / 100
scaleMAG = 0.0625f; // = 1 / 16
scaleGYRO = 0.0625f; // = 1 / 16
scaleEuler = 0.0625f; // = 1 / 16
scaleTEMP = 1.0f;
scaleLIA = scaleACC;
scaleGV = scaleACC;
scaleQuaternion = 0.00006103515; // = 1 / (2^14)
clkExt = false;
}
/* ==================================================================
* デストラクタ
*/
BOARDC_BNO055::~BOARDC_BNO055(){
delete ctrl;
}
/* ==================================================================
* デフォルト設定での初期化
*/
char BOARDC_BNO055::initialize(bool resetIface){
if(resetIface) ctrl->init();
//CONFIGモードに設定
setOperation_CONFIG();
//外部発振子設定,セルフテストの実行,セルフテスト完了まで待つ
setSysTrigger(0x81);
//加速度センサーレンジ設定(+-8G)
//地磁気や角速度センサーのレンジやバンドはFusionモードにより自動設定される
if(!setAccRange(16)) return -1;
//各軸リマップ設定(BNO055の1pinマークが表側右下)
if(!setAxisRemap_topview_bottomright()) return -2;
//単位系セット
//ORI_Android_Windows : Windows
//TEMP_Unit : Celcius
//EUL_Unit : Degree
//GYR_Unit : deg/s
//ACC_Unit : m/s^2
if(!setUNIT_SEL(0x00)) return -3;
//動作モード設定(9軸Fusionモード)
if(!setOperation_Fusion_NDOF()) return -4;
return 0;
}
/* ==================================================================
* 通信で発生した最後のエラー番号を返す
*/
char BOARDC_BNO055::getIfaceLastError(){
return ctrl->getLastError();
}
/* ==================================================================
* 通信で発生した最後のエラーの通信バイト数を返す
*/
char BOARDC_BNO055::getIfaceLastLength(){
return ctrl->getLastLength();
}
/* ==================================================================
* ユーザー定義読み取り(1byte)
* レジスタを指定して値を直接読み取る
*/
char BOARDC_BNO055::customRead(bool isPage1, char regAddr){
return ctrl->rr(isPage1, regAddr);
}
/* ==================================================================
* ユーザー定義読み取り(連続)
* レジスタを指定して値を直接読み取る
*/
char BOARDC_BNO055::customReadC(bool isPage1, char startRegAddr, unsigned char *receiveBytes, unsigned char length){
return ctrl->rrc(isPage1, startRegAddr, receiveBytes, length);
}
/* ==================================================================
* ユーザー定義書き込み(1byte)
* レジスタを指定して値を直接書き込む
*/
char BOARDC_BNO055::customWrite(bool isPage1, char regAddr, char wBytes){
return ctrl->wr(isPage1, regAddr, wBytes);
}
/* ==================================================================
* ユーザー定義書き込み(連続)
* レジスタを指定して値を直接書き込む
*/
char BOARDC_BNO055::customWriteC(bool isPage1, char startRegAddr, char *Bytes, unsigned char length){
return ctrl->wrc(isPage1, startRegAddr, Bytes, length);
}
/* ==================================================================
* レジスタ：ページIDの現在値を読む
* 実際はコントロールクラスが保持している内部変数の値を返しているだけ
*/
char BOARDC_BNO055::getPage(){
return ctrl->getNowPage();
}
/* ==================================================================
* レジスタ：ページIDを変更する
* ページIDが現在値と同じであれば何もしない
*/
void BOARDC_BNO055::setPage(unsigned char pageNo){
pageNo = (pageNo == 0) ? 0 : 1;
if(getPage() == pageNo) return;
ctrl->wr((getPage() == 0) ? true : false, BNO055_PAGE_ID, pageNo);
}
/* ==================================================================
* レジスタ：チップIDの値を取得する
*/
char BOARDC_BNO055::getChipID(){
return ctrl->rr(0, BNO055P0_CHIP_ID);
}
/* ==================================================================
* レジスタ：加速度センサーIDの値を取得する
*/
char BOARDC_BNO055::getAccChipID(){
return ctrl->rr(0, BNO055P0_ACC_ID);
}
/* ==================================================================
* レジスタ：地磁気センサーIDの値を取得する
*/
char BOARDC_BNO055::getMagChipID(){
return ctrl->rr(0, BNO055P0_MAG_ID);
}
/* ==================================================================
* レジスタ：ジャイロセンサーIDの値を取得する
*/
char BOARDC_BNO055::getGyroChipID(){
return ctrl->rr(0, BNO055P0_GYR_ID);
}
/* ==================================================================
* レジスタ：内部ソフトウェアリビジョンの値を取得する
*/
short BOARDC_BNO055::getRevision(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_SW_REV_ID_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* レジスタ：内部ブートリビジョンの値を取得する
*/
char BOARDC_BNO055::getBootRevision(){
return ctrl->rr(0, BNO055P0_BL_REV_ID);
}
/* ==================================================================
* 加速度センサーのRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getAccScale(){
return scaleACC;
}
/* ==================================================================
* 地磁気センサーのRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getMagScale(){
return scaleMAG;
}
/* ==================================================================
* 角速度センサーのRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getGyroScale(){
return scaleGYRO;
}
/* ==================================================================
* 温度センサーのRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getTempScale(){
return scaleTEMP;
}
/* ==================================================================
* オイラー角のRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getEulerScale(){
return scaleEuler;
}
/* ==================================================================
* 加速度センサーの線形加速度のRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getLinearScale(){
return scaleLIA;
}
/* ==================================================================
* 重力ベクトルのRAW値に乗算する係数を取得する
*/
float BOARDC_BNO055::getGVScale(){
return scaleGV;
}
/* ==================================================================
* 四元数のRAW値に乗算する係数を取得する
*/
double BOARDC_BNO055::getQuaternionScale(){
return scaleQuaternion;
}
/* ==================================================================
* 加速度センサの値を取得する
* ------------------------------------------------------------------
* &accX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
* &accY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される
* &accZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される
*/
void BOARDC_BNO055::getAccDataAll(short &accX, short &accY, short &accZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 6);
accX = (rsv[1] << 8) | rsv[0];
accY = (rsv[3] << 8) | rsv[2];
accZ = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* 加速度センサの値(X軸のみ)を取得する
*/
short BOARDC_BNO055::getAccDataX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 加速度センサの値(Y軸のみ)を取得する
*/
short BOARDC_BNO055::getAccDataY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_DATA_Y_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 加速度センサの値(Z軸のみ)を取得する
*/
short BOARDC_BNO055::getAccDataZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_DATA_Z_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 地磁気センサの値を取得する
* ------------------------------------------------------------------
* &magX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
* &magY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される
* &magZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される
*/
void BOARDC_BNO055::getMagDataAll(short &magX, short &magY, short &magZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 6);
magX = (rsv[1] << 8) | rsv[0];
magY = (rsv[3] << 8) | rsv[2];
magZ = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* 地磁気センサの値(X軸のみ)を取得する
*/
short BOARDC_BNO055::getMagDataX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 地磁気センサの値(Y軸のみ)を取得する
*/
short BOARDC_BNO055::getMagDataY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_DATA_Y_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 地磁気センサの値(Z軸のみ)を取得する
*/
short BOARDC_BNO055::getMagDataZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_DATA_Z_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* ジャイロセンサの値を取得する
* ------------------------------------------------------------------
* &gyroX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
* &gyroY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される
* &gyroZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される
*/
void BOARDC_BNO055::getGyroDataAll(short &gyroX, short &gyroY, short &gyroZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 6);
gyroX = (rsv[1] << 8) | rsv[0];
gyroY = (rsv[3] << 8) | rsv[2];
gyroZ = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* ジャイロセンサの値(X軸のみ)を取得する
*/
short BOARDC_BNO055::getGyroDataX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* ジャイロセンサの値(Y軸のみ)を取得する
*/
short BOARDC_BNO055::getGyroDataY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_DATA_Y_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* ジャイロセンサの値(Z軸のみ)を取得する
*/
short BOARDC_BNO055::getGyroDataZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_DATA_Z_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* FusionSensing:オイラー角の値を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
* Roll軸およびPitch軸が+-45degの範囲を超える場合、Yaw軸の値は不安定となる
* (その場合はgetEulerFromQを使用することで四元数からオイラー角を導出可能)
* ------------------------------------------------------------------
* &E_heading: アドレス参照引数:関数実行後、この変数にYaw軸(heading)の値が格納される
* &E_roll: アドレス参照引数:関数実行後、この変数にroll軸の値が格納される
* &E_pitch: アドレス参照引数:関数実行後、この変数にpitch軸の値が格納される
*/
void BOARDC_BNO055::getEulerDataAll(short &E_heading, short &E_roll, short &E_pitch){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 6);
E_heading = (rsv[1] << 8) | rsv[0];
E_roll = (rsv[3] << 8) | rsv[2];
E_pitch = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* FusionSensing:オイラー角の値(Yaw軸(heading)のみ)を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
*/
short BOARDC_BNO055::getEulerDataHeading(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* getEulerDataHeading()のエイリアス
* FusionSensing:オイラー角の値(Yaw軸(heading)のみ)を返す
*/
short BOARDC_BNO055::getEulerDataYaw(){
return getEulerDataHeading();
}
/* ==================================================================
* FusionSensing:オイラー角の値(pitch軸のみ)を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
*/
short BOARDC_BNO055::getEulerDataRoll(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_EUL_ROLL_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* FusionSensing:オイラー角の値(pitch軸のみ)を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
*/
short BOARDC_BNO055::getEulerDataPitch(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_EUL_PITCH_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* FusionSensing:9軸センサーのすべての値の値を取得する
*/
void BOARDC_BNO055::get9Axis(short *box){
unsigned char rsv[18];
ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 18);
box[0] = (rsv[1] << 8) | rsv[0];
box[1] = (rsv[3] << 8) | rsv[2];
box[2] = (rsv[5] << 8) | rsv[4];
box[3] = (rsv[7] << 8) | rsv[6];
box[4] = (rsv[9] << 8) | rsv[8];
box[5] = (rsv[11] << 8) | rsv[10];
box[6] = (rsv[13] << 8) | rsv[12];
box[7] = (rsv[15] << 8) | rsv[14];
box[8] = (rsv[17] << 8) | rsv[16];
}
/* ==================================================================
* FusionSensing:9軸センサーのすべての値とオイラー角のすべての値を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
*/
void BOARDC_BNO055::get9AxisAndEUL(short *box){
unsigned char rsv[24];
ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 24);
box[0] = (rsv[1] << 8) | rsv[0];
box[1] = (rsv[3] << 8) | rsv[2];
box[2] = (rsv[5] << 8) | rsv[4];
box[3] = (rsv[7] << 8) | rsv[6];
box[4] = (rsv[9] << 8) | rsv[8];
box[5] = (rsv[11] << 8) | rsv[10];
box[6] = (rsv[13] << 8) | rsv[12];
box[7] = (rsv[15] << 8) | rsv[14];
box[8] = (rsv[17] << 8) | rsv[16];
box[9] = (rsv[19] << 8) | rsv[18];
box[10] = (rsv[21] << 8) | rsv[20];
box[11] = (rsv[23] << 8) | rsv[22];
}
/* ==================================================================
* FusionSensing:四元数(Quaternion)を取得する
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
* ------------------------------------------------------------------
* &q1, &q2, &q3, &q4:アドレス参照引数:関数実行後、この変数に四元数が格納される
*/
void BOARDC_BNO055::getQuaternion(short &q1, short &q2, short &q3, short &q4){
//連続8byte読み取り
unsigned char rsv[8];
ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8);
q1 = (rsv[1] << 8) | rsv[0];
q2 = (rsv[3] << 8) | rsv[2];
q3 = (rsv[5] << 8) | rsv[4];
q4 = (rsv[7] << 8) | rsv[6];
}
/* ==================================================================
* FusionSensing:四元数(Quaternion)を取得し、オイラー角を算出して返す
* FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す
* 出力数値範囲 -180.0 to 180.0[deg] CW(時計回り)- CCW(反時計回り)+
* ------------------------------------------------------------------
* &E_heading, &E_roll, &E_pitch:アドレス参照引数:関数実行後、この変数にオイラー角が格納される
*/
void BOARDC_BNO055::getEulerFromQ(double &E_heading, double &E_roll, double &E_pitch){
//連続8byte読み取り
unsigned char rsv[8];
ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8);
//四元数を実際の数値に変換(1 / 2^14を掛けるより2^14で割ったほうが早い・・・)
double q1, q2, q3, q4;
q1 = (double)((short)((rsv[1] << 8) | rsv[0]) / 16384.0);
q2 = (double)((short)((rsv[3] << 8) | rsv[2]) / 16384.0);
q3 = (double)((short)((rsv[5] << 8) | rsv[4]) / 16384.0);
q4 = (double)((short)((rsv[7] << 8) | rsv[6]) / 16384.0);
//四元数からオイラー角に変換
//ref: Wikipedia
//Conversion between quaternions and Euler angles(Quaternion to Euler Angles Conversion)
double q3q3 = q3 * q3;
double m1 = +2.0 * (q1 * q2 + q3 * q4);
double m2 = +1.0 - 2.0 * (q2 * q2 + q3q3);
E_roll = atan2(m1, m2) * 57.2957795131;
m1 = +2.0 * (q1 * q3 - q4 * q2);
m1 = (m1 > 1.0)? 1.0 : m1;
m1 = (m1 < -1.0)? -1.0 : m1;
E_pitch = asin(m1) * 57.2957795131;
m1 = +2.0 * (q1 * q4 + q2 * q3);
m2 = +1.0 - 2.0 * (q3q3 + q4 * q4);
E_heading = atan2(m1, m2) * 57.2957795131;
}
/* ==================================================================
* 線形加速度(LinearAcceleration)での加速度センサの値を取得する
* ------------------------------------------------------------------
* &L_accX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
* &L_accY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される
* &L_accZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される
*/
void BOARDC_BNO055::getLinearAccDataAll(short &L_accX, short &L_accY, short &L_accZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 6);
L_accX = (rsv[1] << 8) | rsv[0];
L_accY = (rsv[3] << 8) | rsv[2];
L_accZ = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* 線形加速度(LinearAcceleration)での加速度センサの値(X軸のみ)を取得する
*/
short BOARDC_BNO055::getLinearAccDataX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 線形加速度(LinearAcceleration)での加速度センサの値(Y軸のみ)を取得する
*/
short BOARDC_BNO055::getLinearAccDataY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_LIA_DATA_Y_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 線形加速度(LinearAcceleration)での加速度センサの値(Z軸のみ)を取得する
*/
short BOARDC_BNO055::getLinearAccDataZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_LIA_DATA_Z_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 重力ベクトル情報を取得する
* ------------------------------------------------------------------
* &gvX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される
* &gvY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される
* &gvZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される
*/
void BOARDC_BNO055::getGVectorDataAll(short &gvX, short &gvY, short &gvZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 6);
gvX = (rsv[1] << 8) | rsv[0];
gvY = (rsv[3] << 8) | rsv[2];
gvZ = (rsv[5] << 8) | rsv[4];
}
/* ==================================================================
* 重力ベクトル情報(X軸のみ)を取得する
*/
short BOARDC_BNO055::getGVectorDataX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 重力ベクトル情報(Y軸のみ)を取得する
*/
short BOARDC_BNO055::getGVectorDataY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GRV_DATA_Y_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 重力ベクトル情報(Z軸のみ)を取得する
*/
short BOARDC_BNO055::getGVectorDataZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GRV_DATA_Z_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* センサー内部温度情報を取得する
* 内部温度のセンサーは2つあり、どちらか一方のみの温度を返す
* (設定レジスタTEMP_SOURCEにて選択可)
* (setTempSource(bool)にて選択可)
*/
char BOARDC_BNO055::getTemperature(){
return ctrl->rr(0, BNO055P0_TEMP);
}
/* ==================================================================
* キャリブレーション(補正)の状態を取得する
* ------------------------------------------------------------------
* &sys: アドレス参照引数:関数実行後、この変数にsystem補正の状態が格納される
* &acc: アドレス参照引数:関数実行後、この変数に加速度センサー補正の状態が格納される
* &mag: アドレス参照引数:関数実行後、この変数に地磁気センサー補正の状態が格納される
* &gyro: アドレス参照引数:関数実行後、この変数に角速度センサー補正の状態が格納される
* ------------------------------------------------------------------
* 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため
* 値は0[%],32[%],66[%],100[%]のいずれか
*/
void BOARDC_BNO055::getCalibStatusAll(char &sys, char &acc, char &mag, char &gyro){
char rv = ctrl->rr(0, BNO055P0_CALIB_STAT);
sys = (((rv & 0xC0) >> 6) * 34);
gyro = (((rv & 0x30) >> 4) * 34);
acc = (((rv & 0x0C) >> 2) * 34);
mag = ((rv & 0x03) * 34);
sys -= (sys == 0) ? 0 : 2;
gyro -= (gyro == 0) ? 0 : 2;
acc -= (acc == 0) ? 0 : 2;
mag -= (mag == 0) ? 0 : 2;
}
/* ==================================================================
* キャリブレーション(補正)の状態(systemのみ)を取得する
* ------------------------------------------------------------------
* 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため
* 値は0[%],32[%],66[%],100[%]のいずれか
*/
char BOARDC_BNO055::getCalibStatusSys(){
char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0xC0) >> 6) * 34) - 2;
return (ret < 0) ? 0 : ret;
}
/* ==================================================================
* キャリブレーション(補正)の状態(加速度センサーのみ)を取得する
* ------------------------------------------------------------------
* 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため
* 値は0[%],32[%],66[%],100[%]のいずれか
*/
char BOARDC_BNO055::getCalibStatusAcc(){
char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x0C) >> 2) * 34) - 2;
return (ret < 0) ? 0 : ret;
}
/* ==================================================================
* キャリブレーション(補正)の状態(地磁気センサーのみ)を取得する
* ------------------------------------------------------------------
* 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため
* 値は0[%],32[%],66[%],100[%]のいずれか
*/
char BOARDC_BNO055::getCalibStatusMag(){
char ret = ((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x03) * 34) - 2;
return (ret < 0) ? 0 : ret;
}
/* ==================================================================
* キャリブレーション(補正)の状態(角速度センサーのみ)を取得する
* ------------------------------------------------------------------
* 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため
* 値は0[%],32[%],66[%],100[%]のいずれか
*/
char BOARDC_BNO055::getCalibStatusGyro(){
char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x30) >> 4) * 34) - 2;
return (ret < 0) ? 0 : ret;
}
/* ==================================================================
* システムおよびセンサーのセルフテストの実行結果を取得する
* ------------------------------------------------------------------
* returns:
* 1bit目:加速度センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常))
* 2bit目:地磁気センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常))
* 3bit目:角速度センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常))
* 4bit目:内部マイコンのセルフテストの実行結果(0:failed(異常), 1:passed(正常))
* ------------------------------------------------------------------
* 例:戻り値 0x0D (0b00001101)
* >>加速度センサー:正常(1)
* >>地磁気センサー:異常(0)
* >>角速度センサー：正常(1)
* >>内部マイコン:正常(1)
*/
char BOARDC_BNO055::getSelfTestResultAll(){
return ctrl->rr(0, BNO055P0_ST_RESULT);
}
/* ==================================================================
* 内部マイコン(BNO055のMCU)のセルフテストの実行結果を取得する
* ------------------------------------------------------------------
* returns:
* false:failed(異常)
* true:passed(正常)
*/
bool BOARDC_BNO055::getSelfTestResultMCU(){
return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x08) >> 3) == 1);
}
/* ==================================================================
* 加速度センサーのセルフテストの実行結果を取得する
* ------------------------------------------------------------------
* returns:
* false:failed(異常)
* true:passed(正常)
*/
bool BOARDC_BNO055::getSelfTestResultAcc(){
return ((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x01) == 1);
}
/* ==================================================================
* 地磁気センサーのセルフテストの実行結果を取得する
* ------------------------------------------------------------------
* returns:
* false:failed(異常)
* true:passed(正常)
*/
bool BOARDC_BNO055::getSelfTestResultMag(){
return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x02) >> 1) == 1);
}
/* ==================================================================
* 地磁気センサーのセルフテストの実行結果を取得する
* ------------------------------------------------------------------
* returns:
* false:failed(異常)
* true:passed(正常)
*/
bool BOARDC_BNO055::getSelfTestResultGyro(){
return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x04) >> 2) == 1);
}
/* ==================================================================
* 発生している割り込みステータス情報を取得する
* ------------------------------------------------------------------
* returns:
* 3bit目:GYRO_AM(角速度AnyMotion発生)(1:発生中, 0:なし)
* 4bit目:GYR_HIGH_RATE(角速度ハイレート発生)(1:発生中, 0:なし)
* 6bit目:ACC_HIGH_G(加速度急加速発生)(1:発生中, 0:なし)
* 7bit目:ACC_AM(角速度AnyMotion発生)(1:発生中, 0:なし)
* 8bit目:ACC_NM(角速度NoMotion発生)(1:発生中, 0:なし)
*/
char BOARDC_BNO055::triggeredIntALL(){
return ctrl->rr(0, BNO055P0_INT_STA);
}
/* ==================================================================
* 発生している割り込みステータス情報(ACC_NM)を取得する
* ------------------------------------------------------------------
* returns:
* true:ACC_NMトリガー発生中
* false:ACC_NMトリガーなし
*/
bool BOARDC_BNO055::triggeredACC_NM(){
return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x80) >> 7) == 1);
}
/* ==================================================================
* 発生している割り込みステータス情報(ACC_AM)を取得する
* ------------------------------------------------------------------
* returns:
* true:ACC_AMトリガー発生中
* false:ACC_AMトリガーなし
*/
bool BOARDC_BNO055::triggeredACC_AM(){
return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x40) >> 6) == 1);
}
/* ==================================================================
* 発生している割り込みステータス情報(ACC_HIGH_G)を取得する
* ------------------------------------------------------------------
* returns:
* true:ACC_HIGH_Gトリガー発生中
* false:ACC_HIGH_Gトリガーなし
*/
bool BOARDC_BNO055::triggeredACC_HIGH_G(){
return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x20) >> 5) == 1);
}
/* ==================================================================
* 発生している割り込みステータス情報(GYR_HIGH_RATE)を取得する
* ------------------------------------------------------------------
* returns:
* true:GYR_HIGH_RATEトリガー発生中
* false:GYR_HIGH_RATEトリガーなし
*/
bool BOARDC_BNO055::triggeredGYR_HIGH_RATE(){
return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x08) >> 3) == 1);
}
/* ==================================================================
* 発生している割り込みステータス情報(GYRO_AM)を取得する
* ------------------------------------------------------------------
* returns:
* true:GYRO_AMトリガー発生中
* false:GYRO_AMトリガーなし
*/
bool BOARDC_BNO055::triggeredGYRO_AM(){
return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x04) >> 2) == 1);
}
/* ==================================================================
* BNO055のシステムクロック固定情報を取得する
* ------------------------------------------------------------------
* returns:
* true:設定によって固定されている(SYS_TRIGGEレジスタのCLK_SEL)
* false:設定されていない(内部か外部選択可)
* ------------------------------------------------------------------
* setSys_ExternalCrystal(bool)で設定可
*/
bool BOARDC_BNO055::isSystemClockFixed(){
return (ctrl->rr(0, BNO055P0_SYS_CLK_STATUS) == 1);
}
/* ==================================================================
* BNO055のシステムステータスを取得する
* ------------------------------------------------------------------
* returns:
* 0: システム待機状態
* 1: システムエラー
* 2: ペリフェラル初期化中
* 3: システム初期化中
* 4: セルフテスト実行中
* 5: 起動中(Fusionアルゴリズム起動中)
* 6: 起動中(Fusionアルゴリズムなし)
*/
char BOARDC_BNO055::getSystemStatus(){
return ctrl->rr(0, BNO055P0_SYS_STATUS);
}
/* ==================================================================
* BNO055のシステムエラー情報を取得する
* ------------------------------------------------------------------
* returns:
* 0: エラーなし
* 1: ペリフェラル初期化エラー
* 2: システム初期化エラー
* 3: セルフテスト結果不調
* 4: レジスタマップエラー(値の範囲外)
* 5: レジスタマップエラー(アドレスの範囲外)
* 6: レジスタマップエラー(書き込み不能)
* 7: BNO005LowPowerモードにつき指定モード実行不可
* 8: 加速度センサーPowerMode実行不可
* 9: Fusionアルゴリズム設定エラー
* 10: 各センサー設定エラー
*/
char BOARDC_BNO055::getSystemError(){
return ctrl->rr(0, BNO055P0_SYS_ERR);
}
/* ==================================================================
* システムの設定単位系を取得する
* ------------------------------------------------------------------
* returns:
* 1bit目: 加速度センサー単位系(0:m/s^2, 1:mg)
* 2bit目: 角速度センサー単位系(0:deg/s, 1:rad/s)
* 3bit目: オイラー角単位系(0:deg, 1:rad)
* 5bit目: 温度単位系(0:摂氏, 1:華氏)
* 7bit目: 出力設定(0:Windows, 1:Android)
*/
char BOARDC_BNO055::getUNIT_SEL(){
return ctrl->rr(0, BNO055P0_UNIT_SEL);
}
/* ==================================================================
* システムの設定単位系を設定する
* ------------------------------------------------------------------
* 引数selectValue:
* 1bit目: 加速度センサー単位系(0:m/s^2, 1:mg)
* 2bit目: 角速度センサー単位系(0:deg/s, 1:rad/s)
* 3bit目: オイラー角単位系(0:deg, 1:rad)
* 5bit目: 温度単位系(0:摂氏, 1:華氏)
* 7bit目: 出力設定(0:Windows, 1:Android)
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_SEL(char selectValue){
if(ctrl->wr(0, BNO055P0_UNIT_SEL, selectValue) == -1) return -1;
scaleACC = ((selectValue & 0x01) == 0) ? 0.01f : 1.0f;
scaleGYRO = ((selectValue & 0x02) == 0) ? 0.0625f : 0.001111111111f;
scaleEuler = ((selectValue & 0x04) == 0) ? 0.0625f : 0.001111111111f;
scaleTEMP = ((selectValue & 0x10) == 0) ? 1.0f : 2.0f;
scaleLIA = scaleACC;
scaleGV = scaleACC;
return 1;
}
/* ==================================================================
* システムの設定単位系(加速度センサー)を設定する
* ------------------------------------------------------------------
* 引数isMeterPerSec2:
* true: m/s^2
* false: mg
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_AccUnit(bool isMeterPerSec2){
char val = getUNIT_SEL() & 0xFE;
if(!isMeterPerSec2) val += 1;
return setUNIT_SEL(val);
}
/* ==================================================================
* システムの設定単位系(角速度センサー)を設定する
* ------------------------------------------------------------------
* 引数isDps:
* true: dps(Degrees/s)
* false: rps(Radians/s)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_GyroUnit(bool isDps){
char val = getUNIT_SEL() & 0xFD;
if(!isDps) val += 2;
return setUNIT_SEL(val);
}
/* ==================================================================
* システムの設定単位系(オイラー角)を設定する
* ------------------------------------------------------------------
* 引数isDegrees:
* true: Degrees
* false: Radians
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_EulerUnit(bool isDegrees){
char val = getUNIT_SEL() & 0xFB;
if(!isDegrees) val += 4;
return setUNIT_SEL(val);
}
/* ==================================================================
* システムの設定単位系(温度)を設定する
* ------------------------------------------------------------------
* 引数isCelsius:
* true: 摂氏(Celsius)
* false: 華氏(Fahrenheit)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_Temperature(bool isCelsius){
char val = getUNIT_SEL() & 0xEF;
if(!isCelsius) val += 16;
return setUNIT_SEL(val);
}
/* ==================================================================
* システムの設定単位系(出力設定)を設定する
* ------------------------------------------------------------------
* 引数ori_Android:
* true: Android用(Pitch角:+180 to -180 反時計回り＋)
* false: Windows用(Pitch角:-180 to +180 時計回り＋)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setUNIT_OrientationMode(bool ori_Android){
char val = getUNIT_SEL() & 0xBF;
if(!ori_Android) val += 64;
return setUNIT_SEL(val);
}
/* ==================================================================
* システムの動作モードを取得する
* ------------------------------------------------------------------
* returns:
* 1 ACCONLY(NO FUSION)加速度のみ
* 2 MAGONLY(NO FUSION)地磁気のみ
* 3 GYROONLY(NO FUSION)角速度のみ
* 4 ACCMAG(NO FUSION)加速度と地磁気
* 5 ACCGYRO(NO FUSION)加速度と角速度
* 6 MAGGYRO(NO FUSION)地磁気と角速度
* 7 AMG(NO FUSION)加速度、地磁気、角速度
* 8 IMU
* 9 COMPASS
* 10 M4G
* 11 NDOF_FMC_OFF
* 12 NDOF
*/
char BOARDC_BNO055::getOperationMode(){
return ctrl->rr(0, BNO055P0_OPR_MODE);
}
/* ==================================================================
* システムの動作モードを設定する
* ------------------------------------------------------------------
* 引数modeValue:
* 1 ACCONLY(NO FUSION)加速度のみ
* 2 MAGONLY(NO FUSION)地磁気のみ
* 3 GYROONLY(NO FUSION)角速度のみ
* 4 ACCMAG(NO FUSION)加速度と地磁気
* 5 ACCGYRO(NO FUSION)加速度と角速度
* 6 MAGGYRO(NO FUSION)地磁気と角速度
* 7 AMG(NO FUSION)加速度、地磁気、角速度
* 8 IMU
* 9 COMPASS
* 10 M4G
* 11 NDOF_FMC_OFF
* 12 NDOF
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperationMode(char modeValue){
if(modeValue < 0 || modeValue > 12) modeValue = 7;
return ctrl->wr(0, BNO055P0_OPR_MODE, modeValue);
}
/* ==================================================================
* システムの動作モードを設定モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_CONFIG(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 0);
}
/* ==================================================================
* システムの動作モードを加速度モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_ACCONRY(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 1);
}
/* ==================================================================
* システムの動作モードを地磁気モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_MAGONRY(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 2);
}
/* ==================================================================
* システムの動作モードを角速度モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_GYROONRY(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 3);
}
/* ==================================================================
* システムの動作モードを加速度地磁気モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_ACCMAG(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 4);
}
/* ==================================================================
* システムの動作モードを加速度角速度モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_ACCGYRO(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 5);
}
/* ==================================================================
* システムの動作モードを地磁気角速度モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_MAGGYRO(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 6);
}
/* ==================================================================
* システムの動作モードをFusionなし9軸モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_AMG(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 7);
}
/* ==================================================================
* システムの動作モードを6軸(加速度、角速度)Fusionモードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_Fusion_IMU(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 8);
}
/* ==================================================================
* システムの動作モードを6軸(加速度、地磁気)Fusionモード(相対系)に設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_Fusion_COMPASS(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 9);
}
/* ==================================================================
* システムの動作モードを6軸(加速度、地磁気)Fusionモード(絶対系)に設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_Fusion_M4G(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 10);
}
/* ==================================================================
* システムの動作モードをNDOFモード(地磁気短時間補正OFF)に設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_Fusion_NDOF_FMC_OFF(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 11);
}
/* ==================================================================
* システムの動作モードをNDOFモードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setOperation_Fusion_NDOF(){
return ctrl->wr(0, BNO055P0_OPR_MODE, 12);
}
/* ==================================================================
* システムの電源モードを取得する
* ------------------------------------------------------------------
* returns:
* 0: Normal
* 1: LowPower
* 2: Suspend
*/
char BOARDC_BNO055::getPowerMode(){
return ctrl->rr(0, BNO055P0_PWR_MODE);
}
/* ==================================================================
* システムの電源モードを設定する
* ------------------------------------------------------------------
* modeValue:
* 0: Normal
* 1: LowPower
* 2: Suspend
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setPowerMode(unsigned char modeValue){
if(modeValue > 2) return -1;
return ctrl->wr(0, BNO055P0_PWR_MODE, modeValue);
}
/* ==================================================================
* システムの電源モードを通常モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setPowerMode_Normal(){
return ctrl->wr(0, BNO055P0_PWR_MODE, 0);
}
/* ==================================================================
* システムの電源モードを低消費電力モードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setPowerMode_LowPower(){
return ctrl->wr(0, BNO055P0_PWR_MODE, 1);
}
/* ==================================================================
* システムの電源モードをスリープモードに設定する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setPowerMode_Suspend(){
return ctrl->wr(0, BNO055P0_PWR_MODE, 2);
}
/* ==================================================================
* システムのトリガー設定を変更する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 0 変更なし
* 1 成功
*/
char BOARDC_BNO055::setSysTrigger(char regVal){
clkExt = regVal >> 7;
return ctrl->wr(0, BNO055P0_SYS_TRIGGER, regVal & 0xE1);
}
/* ==================================================================
* システムのクロック発振元を設定する
* 外部を指定する場合は、設定前にXIN,XOUTが発振子に結線されている必要がある
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 0 変更なし
* 1 成功
*/
char BOARDC_BNO055::setSys_ExternalCrystal(bool isExternal){
if(clkExt == isExternal) return 0;
clkExt = isExternal;
return ctrl->wr(0, BNO055P0_SYS_TRIGGER, (clkExt) ? 0x80 : 0x00);
}
/* ==================================================================
* システムの割り込み発生をすべてリセットする
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::resetInterrupt(){
return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x40);
}
/* ==================================================================
* システムをリセットする
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::soft_reset(){
return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x20);
}
/* ==================================================================
* セルフテストを実行する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::execSelfTest(){
return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x01);
}
/* ==================================================================
* システムの温度計測に使用するセンサーを取得する
* ------------------------------------------------------------------
* returns:
* 0 温度計測に加速度センサーを使用している
* 1 温度計測に角速度センサーを使用している
*/
char BOARDC_BNO055::getTempSource(){
return ctrl->rr(0, BNO055P0_TEMP_SOURCE);
}
/* ==================================================================
* システムの温度計測に使用するセンサーを選択する
* ------------------------------------------------------------------
* 引数Accelerometer:
* true:加速度センサーを温度計測に使用する
* false:角速度センサーを温度計測に使用する
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setTempSource(bool Accelerometer){
return ctrl->wr(0, BNO055P0_TEMP_SOURCE, (Accelerometer) ? 0 : 1);
}
/* ==================================================================
* センサー出力軸交換情報の取得
* ------------------------------------------------------------------
* returns:
* AXIS_MAP_CONFIGの設定情報:BNO055データシート [3.4 Axis remap] を参照
*/
char BOARDC_BNO055::getAxisMapConfig(){
char ret = ctrl->rr(0, BNO055P0_AXIS_MAP_CONFIG);
if(axisRemap != ret) axisRemap = ret;
return axisRemap;
}
/* ==================================================================
* センサー出力軸交換の設定
* ------------------------------------------------------------------
* 引数val:
* X,Y,Zの出力交換の情報
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 0 変更なし
* 1 成功
*/
char BOARDC_BNO055::setAxisMapConfig(char val){
if(axisRemap == (val & 0x3F)) return 0;
else axisRemap = (val & 0x3F);
return ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
}
/* ==================================================================
* センサー出力軸符号情報の取得
* ------------------------------------------------------------------
* returns:
* AXIS_MAP_SIGNの設定情報:BNO055データシート [3.4 Axis remap] を参照
*/
char BOARDC_BNO055::getAxisMapSign(){
char ret = ctrl->rr(0, BNO055P0_AXIS_MAP_SIGN);
if(axisSign != ret) axisSign = ret;
return axisSign;
}
/* ==================================================================
* センサー出力軸符号の設定
* ------------------------------------------------------------------
* 引数val:
* X,Y,Zの出力符号の情報
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 0 変更なし
* 1 成功
*/
char BOARDC_BNO055::setAxisMapSign(char val){
if(axisSign == (val & 0x07)) return 0;
else axisSign = (val & 0x07);
return ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側左上]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_topview_topleft(){
axisRemap = 0x21;
axisSign = 0x04;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側右上]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_topview_topright(){
axisRemap = 0x24;
axisSign = 0x00;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側左下]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_topview_bottomleft(){
axisRemap = 0x24;
axisSign = 0x06;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側右下]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_topview_bottomright(){
axisRemap = 0x21;
axisSign = 0x02;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側左上]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_bottomview_topleft(){
axisRemap = 0x24;
axisSign = 0x03;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側右上]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_bottomview_topright(){
axisRemap = 0x21;
axisSign = 0x01;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側左下]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_bottomview_bottomleft(){
axisRemap = 0x21;
axisSign = 0x07;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側右下]
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* -1 AXIS_MAP_CONFIG設定の失敗
* -2 AXIS_MAP_SIGN設定の失敗
* 1 成功
*/
char BOARDC_BNO055::setAxisRemap_bottomview_bottomright(){
axisRemap = 0x24;
axisSign = 0x05;
char ret = 0;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap);
if(ret == -1) return -1;
ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign);
if(ret == -1) return -2;
return 1;
}
/* ==================================================================
* センサー出力軸の符号と交換の情報を直感的な形で取得する
* ------------------------------------------------------------------
* (BNO055データシート [3.4 Axis remap] を参照
* ------------------------------------------------------------------
* returns:
* 0 [P0]表側から見たとき、BNO055の1pinが表側左上にある状態
* 1 [P1]表側から見たとき、BNO055の1pinが表側右上にある状態
* 2 [P2]表側から見たとき、BNO055の1pinが表側左下にある状態
* 3 [P3]表側から見たとき、BNO055の1pinが表側右下にある状態
* 4 [P4]表側から見たとき、BNO055の1pinが裏側左上にある状態
* 5 [P5]表側から見たとき、BNO055の1pinが裏側右上にある状態
* 6 [P6]表側から見たとき、BNO055の1pinが裏側左下にある状態
* 7 [P7]表側から見たとき、BNO055の1pinが裏側右下にある状態
* -1 それ以外の設定
*/
char BOARDC_BNO055::getAxisRemap_type(){
getAxisMapConfig(); //axisRemapに最新の値を格納
getAxisMapSign(); //axisSignに最新の値を格納
if(axisRemap == 0x21){
switch(axisSign){
case 0x04:
return 0;
case 0x02:
return 3;
case 0x01:
return 5;
case 0x07:
return 6;
}
}else if(axisRemap == 0x24){
switch(axisSign){
case 0x00:
return 1;
case 0x06:
return 2;
case 0x03:
return 4;
case 0x05:
return 7;
}
}
return -1;
}
/* ==================================================================
* 加速度センサーの補正値を取得する
* ------------------------------------------------------------------
* &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される
* &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される
* &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される
*/
void BOARDC_BNO055::getAccOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 6);
short offX = (rsv[1] << 8) | rsv[0];
short offY = (rsv[3] << 8) | rsv[2];
short offZ = (rsv[5] << 8) | rsv[4];
offsetX = (1.0f * offX) * scaleACC;
offsetY = (1.0f * offY) * scaleACC;
offsetZ = (1.0f * offZ) * scaleACC;
}
/* ==================================================================
* 加速度センサーの補正値(X軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* X軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getAccOffsetX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 2);
short offX = (rsv[1] << 8) | rsv[0];
return (1.0f * offX) * scaleACC;
}
/* ==================================================================
* 加速度センサーの補正値(Y軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Y軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getAccOffsetY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_OFFSET_Y_LSB, rsv, 2);
short offX = (rsv[1] << 8) | rsv[0];
return (1.0f * offX) * scaleACC;
}
/* ==================================================================
* 加速度センサーの補正値(Z軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Z軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getAccOffsetZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_OFFSET_Z_LSB, rsv, 2);
short offX = (rsv[1] << 8) | rsv[0];
return (1.0f * offX) * scaleACC;
}
/* ==================================================================
* 加速度センサーの補正値を設定する
* ------------------------------------------------------------------
* offsetX: スケール乗算済みのX軸の補正値
* offsetY: スケール乗算済みのY軸の補正値
* offsetZ: スケール乗算済みのZ軸の補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccOffsetAll(float offsetX, float offsetY, float offsetZ){
short offX = (short)((offsetX / scaleACC) + 0.5f);
short offY = (short)((offsetY / scaleACC) + 0.5f);
short offZ = (short)((offsetZ / scaleACC) + 0.5f);
char msg[6];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
msg[2] = offY & 0xFF;
msg[3] = offY >> 8;
msg[4] = offZ & 0xFF;
msg[5] = offZ >> 8;
//連続6byte書き込み
return ctrl->wrc(0, BNO055P0_ACC_OFFSET_X_LSB, msg, 6);
}
char BOARDC_BNO055::setAccOffsetX(float offset){
short offX = (short)((offset / scaleACC) + 0.5f);
char msg[2];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_ACC_OFFSET_X_LSB, msg, 2);
}
char BOARDC_BNO055::setAccOffsetY(float offset){
short offY = (short)((offset / scaleACC) + 0.5f);
char msg[2];
msg[0] = offY & 0xFF;
msg[1] = offY >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_ACC_OFFSET_Y_LSB, msg, 2);
}
char BOARDC_BNO055::setAccOffsetZ(float offset){
short offZ = (short)((offset / scaleACC) + 0.5f);
char msg[2];
msg[0] = offZ & 0xFF;
msg[1] = offZ >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_ACC_OFFSET_Z_LSB, msg, 2);
}
/* ==================================================================
* 地磁気センサーの補正値を取得する
* ------------------------------------------------------------------
* &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される
* &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される
* &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される
*/
void BOARDC_BNO055::getMagOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 6);
short offX = (rsv[1] << 8) | rsv[0];
short offY = (rsv[3] << 8) | rsv[2];
short offZ = (rsv[5] << 8) | rsv[4];
offsetX = (1.0f * offX) * scaleMAG;
offsetY = (1.0f * offY) * scaleMAG;
offsetZ = (1.0f * offZ) * scaleMAG;
}
/* ==================================================================
* 地磁気センサーの補正値(X軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* X軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getMagOffsetX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 2);
short offX = (rsv[1] << 8) | rsv[0];
return (1.0f * offX) * scaleMAG;
}
/* ==================================================================
* 地磁気センサーの補正値(Y軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Y軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getMagOffsetY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_OFFSET_Y_LSB, rsv, 2);
short offY = (rsv[1] << 8) | rsv[0];
return (1.0f * offY) * scaleMAG;
}
/* ==================================================================
* 地磁気センサーの補正値(Z軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Z軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getMagOffsetZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_OFFSET_Z_LSB, rsv, 2);
short offZ = (rsv[1] << 8) | rsv[0];
return (1.0f * offZ) * scaleMAG;
}
/* ==================================================================
* 地磁気センサーの補正値を設定する
* ------------------------------------------------------------------
* offsetX: スケール乗算済みのX軸の補正値
* offsetY: スケール乗算済みのY軸の補正値
* offsetZ: スケール乗算済みのZ軸の補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagOffsetAll(float offsetX, float offsetY, float offsetZ){
short offX = (short)((offsetX / scaleMAG) + 0.5f);
short offY = (short)((offsetY / scaleMAG) + 0.5f);
short offZ = (short)((offsetZ / scaleMAG) + 0.5f);
char msg[6];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
msg[2] = offY & 0xFF;
msg[3] = offY >> 8;
msg[4] = offZ & 0xFF;
msg[5] = offZ >> 8;
//連続6byte書き込み
return ctrl->wrc(0, BNO055P0_MAG_OFFSET_X_LSB, msg, 6);
}
/* ==================================================================
* 地磁気センサーの補正値(X軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagOffsetX(float offset){
short offX = (short)((offset / scaleMAG) + 0.5f);
char msg[2];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_MAG_OFFSET_X_LSB, msg, 2);
}
/* ==================================================================
* 地磁気センサーの補正値(Y軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagOffsetY(float offset){
short offY = (short)((offset / scaleMAG) + 0.5f);
char msg[2];
msg[0] = offY & 0xFF;
msg[1] = offY >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_MAG_OFFSET_Y_LSB, msg, 2);
}
/* ==================================================================
* 地磁気センサーの補正値(Z軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagOffsetZ(float offset){
short offZ = (short)((offset / scaleMAG) + 0.5f);
char msg[2];
msg[0] = offZ & 0xFF;
msg[1] = offZ >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_MAG_OFFSET_Z_LSB, msg, 2);
}
/* ==================================================================
* 角速度センサーの補正値を取得する
* ------------------------------------------------------------------
* &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される
* &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される
* &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される
*/
void BOARDC_BNO055::getGyroOffsetAll(float &offsetX, float &offsetY, float &offsetZ){
//連続6byte読み取り
unsigned char rsv[6];
ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 6);
short offX = (rsv[1] << 8) | rsv[0];
short offY = (rsv[3] << 8) | rsv[2];
short offZ = (rsv[5] << 8) | rsv[4];
offsetX = (1.0f * offX) * scaleGYRO;
offsetY = (1.0f * offY) * scaleGYRO;
offsetZ = (1.0f * offZ) * scaleGYRO;
}
/* ==================================================================
* 角速度センサーの補正値(X軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* X軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getGyroOffsetX(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 2);
short offX = (rsv[1] << 8) | rsv[0];
return (1.0f * offX) * scaleGYRO;
}
/* ==================================================================
* 角速度センサーの補正値(Y軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Y軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getGyroOffsetY(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_OFFSET_Y_LSB, rsv, 2);
short offY = (rsv[1] << 8) | rsv[0];
return (1.0f * offY) * scaleGYRO;
}
/* ==================================================================
* 角速度センサーの補正値(Z軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* Z軸補正値(スケール乗算済み)
*/
float BOARDC_BNO055::getGyroOffsetZ(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_GYR_OFFSET_Z_LSB, rsv, 2);
short offZ = (rsv[1] << 8) | rsv[0];
return (1.0f * offZ) * scaleGYRO;
}
/* ==================================================================
* 角速度センサーの補正値を設定する
* ------------------------------------------------------------------
* offsetX: スケール乗算済みのX軸の補正値
* offsetY: スケール乗算済みのY軸の補正値
* offsetZ: スケール乗算済みのZ軸の補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroOffsetAll(float offsetX, float offsetY, float offsetZ){
short offX = (short)((offsetX / scaleGYRO) + 0.5f);
short offY = (short)((offsetY / scaleGYRO) + 0.5f);
short offZ = (short)((offsetZ / scaleGYRO) + 0.5f);
char msg[6];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
msg[2] = offY & 0xFF;
msg[3] = offY >> 8;
msg[4] = offZ & 0xFF;
msg[5] = offZ >> 8;
//連続6byte書き込み
return ctrl->wrc(0, BNO055P0_GYR_OFFSET_X_LSB, msg, 6);
}
/* ==================================================================
* 角速度センサーの補正値(X軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroOffsetX(float offset){
short offX = (short)((offset / scaleGYRO) + 0.5f);
char msg[2];
msg[0] = offX & 0xFF;
msg[1] = offX >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_GYR_OFFSET_X_LSB, msg, 2);
}
/* ==================================================================
* 角速度センサーの補正値(Y軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroOffsetY(float offset){
short offY = (short)((offset / scaleGYRO) + 0.5f);
char msg[2];
msg[0] = offY & 0xFF;
msg[1] = offY >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_GYR_OFFSET_Y_LSB, msg, 2);
}
/* ==================================================================
* 角速度センサーの補正値(Z軸のみ)を設定する
* ------------------------------------------------------------------
* offset: スケール乗算済みの補正値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroOffsetZ(float offset){
short offZ = (short)((offset / scaleGYRO) + 0.5f);
char msg[2];
msg[0] = offZ & 0xFF;
msg[1] = offZ >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_GYR_OFFSET_Z_LSB, msg, 2);
}
/* ==================================================================
* 加速度センサーのデータ出力範囲[単位:LSB]を取得する
* ------------------------------------------------------------------
* returns:
* +-データ計測範囲[単位:LSB]
*/
short BOARDC_BNO055::getAccRadius(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_ACC_RADIUS_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 加速度センサーのデータ出力範囲[単位:LSB]を設定する
* ------------------------------------------------------------------
* 引数r: データ計測範囲の最大値および最低値(-r から +r の範囲が計測範囲)
* 最大値は1000[LSB]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccRadius(short LSB){
if(LSB > 1000) LSB = 1000;
char msg[2];
msg[0] = LSB & 0xFF;
msg[1] = LSB >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_ACC_RADIUS_LSB, msg, 2);
}
/* ==================================================================
* 地磁気センサーのデータ出力範囲[単位:LSB]を取得する
* ------------------------------------------------------------------
* returns:
* +-データ計測範囲
*/
short BOARDC_BNO055::getMagRadius(){
//連続2byte読み取り
unsigned char rsv[2];
ctrl->rrc(0, BNO055P0_MAG_RADIUS_LSB, rsv, 2);
return (rsv[1] << 8) | rsv[0];
}
/* ==================================================================
* 加速度センサーのデータ出力範囲[単位:LSB]を設定する
* ------------------------------------------------------------------
* 引数r: データ計測範囲の最大値および最低値(-r から +r の範囲が計測範囲)
* 最大値は960[LSB]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagRadius(short LSB){
if(LSB > 960) LSB = 960;
char msg[2];
msg[0] = LSB & 0xFF;
msg[1] = LSB >> 8;
//連続2byte書き込み
return ctrl->wrc(0, BNO055P0_MAG_RADIUS_LSB, msg, 2);
}
/* ==================================================================
* 加速度センサー設定のレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 加速度センサー設定のレジスタ値
*/
char BOARDC_BNO055::getAccConfig(){
return ctrl->rr(1, BNO055P1_ACC_CONFIG);
}
/* ==================================================================
* 加速度センサー設定のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数regVal: 加速度センサー設定のレジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccConfig(char regVal){
return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal);
}
/* ==================================================================
* 加速度センサー設定のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数gRange: 加速度センサーの計測範囲
* 引数bandWidth: 加速度センサー出力レート(Fusion時は自動設定)
* 引数powMode: 加速度センサー電源設定(Fusion時は自動設定)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccConfig(char gRange, char bandWidth, char powMode){
char regVal = (powMode << 5) | (bandWidth << 2) | gRange;
return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal);
}
/* ==================================================================
* 加速度センサーの計測範囲を設定する
* ------------------------------------------------------------------
* 引数G: 加速度センサーの計測範囲(+-2, 4, 8, 16Gのいずれか)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccRange(unsigned char G){
char val = 0;
switch(G){
case 2:
val = 0x00;
break;
case 4:
val = 0x01;
break;
case 8:
val = 0x02;
break;
case 16:
val = 0x03;
break;
default:
val = 0x02;
}
char regVal = ctrl->rr(1, BNO055P1_ACC_CONFIG);
regVal = (regVal & 0xFC) | val;
return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal);
}
/* ==================================================================
* 地磁気センサー設定のレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 地磁気センサー設定のレジスタ値
*/
char BOARDC_BNO055::getMagConfig(){
return ctrl->rr(1, BNO055P1_MAG_CONFIG);
}
/* ==================================================================
* 地磁気センサー設定のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数regVal: 地磁気センサー設定のレジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagConfig(char regVal){
return ctrl->wr(1, BNO055P1_MAG_CONFIG, regVal);
}
/* ==================================================================
* 地磁気センサー設定のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数rate: 地磁気センサー出力レート(Fusion時は自動設定)
* 引数oprMode: 地磁気センサー出力モード(Fusion時は自動設定)
* 引数powMode: 地磁気センサー電源設定(Fusion時は自動設定)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setMagConfig(char rate, char oprMode, char powMode){
char regVal = powMode << 5 | oprMode << 3 | rate;
return ctrl->wr(1, BNO055P1_MAG_CONFIG, regVal);
}
/* ==================================================================
* 角速度センサー設定(0)のレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサー設定(0)のレジスタ値
*/
char BOARDC_BNO055::getGyroConfig_0(){
return ctrl->rr(1, BNO055P1_GYR_CONFIG_0);
}
/* ==================================================================
* 角速度センサー設定(0)のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数regVal: 角速度センサー設定(0)のレジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroConfig_0(char regVal){
return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal);
}
/* ==================================================================
* 角速度センサー設定(0)のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数range: 角速度センサーの計測範囲(Fusion時は自動設定)
* 引数bandWidth: 角速度センサー出力レート(Fusion時は自動設定)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroConfig_0(char range, char bandWidth){
char regVal = bandWidth << 3 | range;
return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal);
}
/* ==================================================================
* 角速度センサー設定(1)のレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサー設定(1)のレジスタ値
*/
char BOARDC_BNO055::getGyroConfig_1(){
return ctrl->rr(1, BNO055P1_GYR_CONFIG_0);
}
/* ==================================================================
* 角速度センサー設定(1)のレジスタ値を設定する
* ------------------------------------------------------------------
* 引数powMode: 角速度センサーの電源設定(Fusion時は自動設定)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroConfig_1(char powMode){
return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, powMode & 0x07);
}
/* ==================================================================
* 角速度センサーの計測範囲を設定する
* ------------------------------------------------------------------
* 引数G: 角速度センサーの計測範囲(2000, 1000, 500, 250, 125dpsのいずれか)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroRange(unsigned short dps){
char val = 0;
switch(dps){
case 2000:
val = 0;
break;
case 1000:
val = 1;
break;
case 500:
val = 2;
break;
case 250:
val = 3;
break;
case 125:
val = 4;
break;
default:
val = 1;
}
char regVal = ctrl->rr(1, BNO055P1_GYR_CONFIG_0);
regVal = (regVal & 0xF8) + val;
return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal);
}
/* ==================================================================
* 加速度センサーのスリープモードのレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 加速度センサーのスリープモードのレジスタ値
*/
char BOARDC_BNO055::getAccSleepConfig(){
return ctrl->rr(1, BNO055P1_ACC_SLEEP_CONFIG);
}
/* ==================================================================
* 加速度センサーのスリープモードのレジスタ値を設定する
* ------------------------------------------------------------------
* 引数regVal: 加速度センサーのスリープモードのレジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccSleepConfig(char regVal){
return ctrl->wr(1, BNO055P1_ACC_SLEEP_CONFIG, regVal);
}
/* ==================================================================
* 加速度センサーのスリープモードのレジスタ値を設定する
* ------------------------------------------------------------------
* 引数duration: 加速度センサーのスリープモードの持続閾値
* 引数mode: 加速度センサーのスリープモード選択
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccSleepConfig(char duration, char mode){
char regVal = duration << 1 | mode;
return ctrl->wr(1, BNO055P1_ACC_SLEEP_CONFIG, regVal);
}
/* ==================================================================
* 角速度センサーのスリープモードのレジスタ値を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサーのスリープモードのレジスタ値
*/
char BOARDC_BNO055::getGyroSleepConfig(){
return ctrl->rr(1, BNO055P1_GYR_SLEEP_CONFIG);
}
/* ==================================================================
* 角速度センサーのスリープモードのレジスタ値を設定する
* ------------------------------------------------------------------
* 引数regVal: 角速度センサーのスリープモードのレジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroSleepConfig(char regVal){
return ctrl->wr(1, BNO055P1_GYR_SLEEP_CONFIG, regVal);
}
/* ==================================================================
* 角速度センサーのスリープモードのレジスタ値を設定する
* ------------------------------------------------------------------
* 引数duration: 角速度センサーの自動スリープモードの持続閾値
* 引数mode: 角速度センサーのスリープモードの持続閾値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroSleepConfig(char autoSleepDuration, char duration){
char regVal = autoSleepDuration << 3 | duration;
return ctrl->wr(1, BNO055P1_GYR_SLEEP_CONFIG, regVal);
}
/* ==================================================================
* 各センサーの割り込みフラグ発生のINTピン出力許可設定を取得する
* ------------------------------------------------------------------
* returns:
* 各センサーの割り込みフラグ有効無効設定のレジスタ値
* 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:許可)
* 3bit目 角速度センサーHighRate割り込み(0:無効, 1:許可)
* 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:許可)
* 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:許可)
* 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:許可)
*/
char BOARDC_BNO055::getInterruptMask(){
return ctrl->rr(1, BNO055P1_INT_MSK);
}
/* ==================================================================
* 各センサーの割り込みフラグ発生のINTピン出力許可設定を設定する
* ------------------------------------------------------------------
* 引数mask: 各センサーの割り込みフラグ有効無効設定のレジスタ値
* 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:許可)
* 3bit目 角速度センサーHighRate割り込み(0:無効, 1:許可)
* 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:許可)
* 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:許可)
* 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:許可)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setInterruptMask(char mask){
return ctrl->wr(1, BNO055P1_INT_MSK, mask);
}
/* ==================================================================
* 各センサーの割り込みフラグ有効無効設定を取得する
* ------------------------------------------------------------------
* returns:
* 各センサーの割り込みフラグ有効無効設定のレジスタ値
* 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:有効)
* 3bit目 角速度センサーHighRate割り込み(0:無効, 1:有効)
* 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:有効)
* 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:有効)
* 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:有効)
*/
char BOARDC_BNO055::getInterruptEnable(){
return ctrl->rr(1, BNO055P1_INT_EN);
}
/* ==================================================================
* 各センサーの割り込みフラグ有効無効設定を設定する
* ------------------------------------------------------------------
* 引数mask: 各センサーの割り込みフラグ有効無効設定のレジスタ値
* 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:有効)
* 3bit目 角速度センサーHighRate割り込み(0:無効, 1:有効)
* 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:有効)
* 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:有効)
* 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:有効)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setInterruptEnable(char mask){
return ctrl->wr(1, BNO055P1_INT_EN, mask);
}
/* ==================================================================
* 加速度センサーのAnyMotion割り込み発生閾値を取得する
* ------------------------------------------------------------------
* 引数ismg: 出力する値の単位の選択(true: mG, false: mm/s^2)
* ------------------------------------------------------------------
* returns:
* 加速度センサーのAnyMotion割り込み発生閾値
*/
float BOARDC_BNO055::getAccAnyMotionThreashold(bool ismg){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 3.91;
break;
case 1:
scale = 7.81;
break;
case 2:
scale = 15.63;
break;
case 3:
scale = 31.25;
break;
}
scale *= (ismg) ? 1.0 : 9.80665;
return (1.0 * ctrl->rr(1, BNO055P1_ACC_AM_THRES)) * scale;
}
/* ==================================================================
* 加速度センサーのAnyMotion割り込み発生閾値を設定する
* ------------------------------------------------------------------
* 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2)
* 引数threashold: 単位変換済みの閾値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccAnyMotionThreashold(bool ismg, float threashold){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 3.91;
break;
case 1:
scale = 7.81;
break;
case 2:
scale = 15.63;
break;
case 3:
scale = 31.25;
break;
}
scale *= (ismg) ? 1.0 : 9.8;
char cTh = (char)((threashold / scale) + 0.5);
return ctrl->wr(1, BNO055P1_ACC_AM_THRES, cTh);
}
/* ==================================================================
* 加速度センサーの割り込み使用設定を取得する
* ------------------------------------------------------------------
* returns:
* 0bit目,1bit目 連続発生閾値([1bit目+0bit目+1]回の発生でフラグON)
* 2bit目 AM/NM_X_AXISトリガー使用(0:使用しない, 1:使用する)
* 3bit目 AM/NM_Y_AXISトリガー使用(0:使用しない, 1:使用する)
* 4bit目 AM/NM_Z_AXISトリガー使用(0:使用しない, 1:使用する)
* 5bit目 HG_X_AXISトリガー使用(0:使用しない, 1:使用する)
* 6bit目 HG_Y_AXISトリガー使用(0:使用しない, 1:使用する)
* 7bit目 HG_Z_AXISトリガー使用(0:使用しない, 1:使用する)
*/
char BOARDC_BNO055::getAccInterruptSettings(){
return ctrl->rr(1, BNO055P1_ACC_INT_SETTINGS);
}
/* ==================================================================
* 加速度センサーの割り込み使用設定を設定する
* ------------------------------------------------------------------
* 引数settings:
* 0bit目,1bit目 連続発生閾値([1bit目+0bit目+1]回の発生でフラグON)
* 2bit目 AM/NM_X_AXISトリガー使用(0:使用しない, 1:使用する)
* 3bit目 AM/NM_Y_AXISトリガー使用(0:使用しない, 1:使用する)
* 4bit目 AM/NM_Z_AXISトリガー使用(0:使用しない, 1:使用する)
* 5bit目 HG_X_AXISトリガー使用(0:使用しない, 1:使用する)
* 6bit目 HG_Y_AXISトリガー使用(0:使用しない, 1:使用する)
* 7bit目 HG_Z_AXISトリガー使用(0:使用しない, 1:使用する)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccInterruptSettings(char settings){
return ctrl->wr(1, BNO055P1_ACC_INT_SETTINGS, settings);
}
/* ==================================================================
* 加速度センサーのHighG割り込み閾値を取得する
* ------------------------------------------------------------------
* returns:
* HighG割り込み発生閾値[単位:ミリ秒](2ms - 512ms)
*/
unsigned short BOARDC_BNO055::getAccHighGduration(){
return (ctrl->rr(1, BNO055P1_ACC_HG_DURATION) + 1) << 1;
}
/* ==================================================================
* 加速度センサーのHighG割り込み継続発生閾値を設定する
* ------------------------------------------------------------------
* 引数ms: HighG割り込み継続発生閾値[単位:ミリ秒](2ms - 512ms)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccHighGduration(short ms){
if(ms > 512 || ms < 2) return -1;
ms = (ms >> 1) - 1;
return ctrl->wr(1, BNO055P1_ACC_HG_DURATION, ms & 0xFF);
}
/* ==================================================================
* 加速度センサーのHighG割り込み発生閾値を取得する
* ------------------------------------------------------------------
* 引数ismg: 出力する値の単位の選択(true: mG, false: mm/s^2)
* ------------------------------------------------------------------
* returns:
* HighG割り込み発生閾値
*/
float BOARDC_BNO055::getAccHighGThreashold(bool ismg){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 7.81;
break;
case 1:
scale = 15.63;
break;
case 2:
scale = 31.25;
break;
case 3:
scale = 62.5;
break;
}
scale *= (ismg) ? 1.0 : 9.8;
return (1.0 * ctrl->rr(1, BNO055P1_ACC_HG_THRES)) * scale;
}
/* ==================================================================
* 加速度センサーのHighG割り込み発生閾値を設定する
* ------------------------------------------------------------------
* 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2)
* 引数threashold: 単位変換済みの閾値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccHighGThreashold(bool ismg, float threashold){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 7.81;
break;
case 1:
scale = 15.63;
break;
case 2:
scale = 31.25;
break;
case 3:
scale = 62.5;
break;
}
scale *= (ismg) ? 1.0 : 9.8;
char cTh = (char)((threashold / scale) + 0.5);
return ctrl->wr(1, BNO055P1_ACC_HG_THRES, cTh);
}
/* ==================================================================
* 加速度センサーのNoMotion割り込み発生閾値を取得する
* ------------------------------------------------------------------
* 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2)
* ------------------------------------------------------------------
* returns:
* NoMotion割り込み発生閾値
*/
float BOARDC_BNO055::getAccNMThreashold(bool ismg){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 3.91;
break;
case 1:
scale = 7.81;
break;
case 2:
scale = 15.63;
break;
case 3:
scale = 31.25;
break;
}
scale *= (ismg) ? 1.0 : 9.8;
return (1.0 * ctrl->rr(1, BNO055P1_ACC_NM_THRES)) * scale;
}
/* ==================================================================
* 加速度センサーのNoMotion割り込み発生閾値を設定する
* ------------------------------------------------------------------
* 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2)
* 引数threashold: 単位変換済みの閾値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccNMThreashold(bool ismg, float threashold){
char rc = getAccConfig();
float scale = 0.0f;
switch(rc & 0x03){
case 0:
scale = 3.91;
break;
case 1:
scale = 7.81;
break;
case 2:
scale = 15.63;
break;
case 3:
scale = 31.25;
break;
}
scale *= (ismg) ? 1.0 : 9.8;
char cTh = (char)((threashold / scale) + 0.5);
return ctrl->wr(1, BNO055P1_ACC_NM_THRES, cTh);
}
/* ==================================================================
* 加速度センサーのNoMotion割り込み設定を取得する
* ------------------------------------------------------------------
* returns:
* NoMotion割り込み設定値
* 1bit目: スローモーション、ノーモーション選択(0:NoMotion, 1:SlowMotion)
* 2 - 7bit目: slo_no_mot _dur設定値(データシート参照)
*/
char BOARDC_BNO055::getAccNMsetting(){
return ctrl->rr(1, BNO055P1_ACC_NM_SET);
}
/* ==================================================================
* 加速度センサーのNoMotion割り込み設定を設定する
* ------------------------------------------------------------------
* 引数setting: NoMotion割り込み設定値
* 1bit目: スローモーション、ノーモーション選択(0:NoMotion, 1:SlowMotion)
* 2 - 7bit目: slo_no_mot _dur設定値(データシート参照)
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccNMsetting(char setting){
return ctrl->wr(1, BNO055P1_ACC_NM_SET, setting);
}
/* ==================================================================
* 角速度センサーの割り込み設定を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサーの割り込み設定レジスタ値
*/
char BOARDC_BNO055::getGyroInterruptSettings(){
return ctrl->rr(1, BNO055P1_GYR_INT_SETING);
}
/* ==================================================================
* 角速度センサーの割り込み設定を設定する
* ------------------------------------------------------------------
* 引数setting: 角速度センサーの割り込み設定レジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroInterruptSettings(char settings){
return ctrl->wr(1, BNO055P1_GYR_INT_SETING, settings);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(X軸のみ)を取得する
* ------------------------------------------------------------------
* 引数setting: 角速度センサーの割り込み設定レジスタ値
*/
char BOARDC_BNO055::getGyroHighRateXsetting(){
return ctrl->rr(1, BNO055P1_GYR_HR_X_SET);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(X軸のみ、実際の値)を取得する
* ------------------------------------------------------------------
* 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される
* 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される
*/
void BOARDC_BNO055::getGyroHighRateXsetting_dps(float &hyst, float &thres){
char r = getGyroConfig_0();
hyst = 0.0f;
thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char val = ctrl->rr(1, BNO055P1_GYR_HR_X_SET);
hyst *= (float)((val & 0x60) * 1.0);
thres *= (float)((val & 0x1F) * 1.0);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(X軸のみ)を設定する
* ------------------------------------------------------------------
* 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateXsetting(char setting){
return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, setting);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(X軸のみ)を設定する
* ------------------------------------------------------------------
* 引数hystVal: ヒステリシス
* 引数thresVal: スレッショルド
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateXsetting_dps(float hystVal, float thresVal){
char r = getGyroConfig_0();
float hyst = 0.0f;
float thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char hystChar = (char)((hystVal / hyst) + 0.5);
char thresChar = (char)((thresVal / thres) + 0.5);
return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, ((hystChar << 5) | thresChar));
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(X軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* HighRate割り込み継続発生閾値[単位:ミリ秒]
*/
float BOARDC_BNO055::getGyroHighRateXduration(){
return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_X)) * 2.5;
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(X軸のみ)を設定する
* ------------------------------------------------------------------
* 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateXduration(float duration){
return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, (char)(((duration / 2.5) - 1.0) + 0.5));
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Y軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサーのHighRate割り込み設定
*/
char BOARDC_BNO055::getGyroHighRateYsetting(){
return ctrl->rr(1, BNO055P1_GYR_HR_Y_SET);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Y軸のみ、実際の値)を取得する
* ------------------------------------------------------------------
* 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される
* 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される
*/
void BOARDC_BNO055::getGyroHighRateYsetting_dps(float &hyst, float &thres){
char r = getGyroConfig_0();
hyst = 0.0f;
thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char val = ctrl->rr(1, BNO055P1_GYR_HR_Y_SET);
hyst *= (float)((val & 0x60) * 1.0);
thres *= (float)((val & 0x1F) * 1.0);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Y軸のみ)を設定する
* ------------------------------------------------------------------
* 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateYsetting(char setting){
return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, setting);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Y軸のみ)を設定する
* ------------------------------------------------------------------
* 引数hystVal: ヒステリシス
* 引数thresVal: スレッショルド
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateYsetting_dps(float hystVal, float thresVal){
char r = getGyroConfig_0();
float hyst = 0.0f;
float thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char hystChar = (char)((hystVal / hyst) + 0.5);
char thresChar = (char)((thresVal / thres) + 0.5);
return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, ((hystChar << 5) | thresChar));
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(Y軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* HighRate割り込み継続発生閾値[単位:ミリ秒]
*/
float BOARDC_BNO055::getGyroHighRateYduration(){
return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_Y)) * 2.5;
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(Y軸のみ)を設定する
* ------------------------------------------------------------------
* 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateYduration(float duration){
return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, (char)(((duration / 2.5) - 1.0) + 0.5));
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Z軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* 角速度センサーのHighRate割り込み設定レジスタ値
*/
char BOARDC_BNO055::getGyroHighRateZsetting(){
return ctrl->rr(1, BNO055P1_GYR_HR_Z_SET);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Z軸のみ、実際の値)を取得する
* ------------------------------------------------------------------
* 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される
* 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される
*/
void BOARDC_BNO055::getGyroHighRateZsetting_dps(float &hyst, float &thres){
char r = getGyroConfig_0();
hyst = 0.0f;
thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char val = ctrl->rr(1, BNO055P1_GYR_HR_Z_SET);
hyst *= (float)((val & 0x60) * 1.0);
thres *= (float)((val & 0x1F) * 1.0);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Z軸のみ)を設定する
* ------------------------------------------------------------------
* 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateZsetting(char setting){
return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, setting);
}
/* ==================================================================
* 角速度センサーのHighRate割り込み設定(Z軸のみ)を設定する
* ------------------------------------------------------------------
* 引数hystVal: ヒステリシス
* 引数thresVal: スレッショルド
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateZsetting_dps(float hystVal, float thresVal){
char r = getGyroConfig_0();
float hyst = 0.0f;
float thres = 0.0f;
switch(r & 0x07){
case 0:
hyst = 62.26;
thres = 62.5;
break;
case 1:
hyst = 31.13;
thres = 31.25;
break;
case 2:
hyst = 15.56;
thres = 15.62;
break;
case 3:
hyst = 7.78;
thres = 7.81;
break;
case 4:
hyst = 3.89;
thres = 3.90;
break;
}
char hystChar = (char)((hystVal / hyst) + 0.5);
char thresChar = (char)((thresVal / thres) + 0.5);
return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, ((hystChar << 5) | thresChar));
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(Z軸のみ)を取得する
* ------------------------------------------------------------------
* returns:
* HighRate割り込み継続発生閾値[単位:ミリ秒]
*/
float BOARDC_BNO055::getGyroHighRateZduration(){
return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_Z)) * 2.5;
}
/* ==================================================================
* 角速度センサーのHighRate割り込み継続発生閾値(Z軸のみ)を設定する
* ------------------------------------------------------------------
* 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroHighRateZduration(float duration){
return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, (char)(((duration / 2.5) - 1.0) + 0.5));
}
/* ==================================================================
* 角速度センサーのAnyMotion割り込み閾値を取得する
* ------------------------------------------------------------------
* returns:
* AnyMotion割り込み閾値[単位:dps]
*/
float BOARDC_BNO055::getGyroAnyMotionThreashold(){
char r = getGyroConfig_0();
float scale = 0.0f;
switch(r & 0x07){
case 0:
scale = 1.0f;
break;
case 1:
scale = 0.5;
break;
case 2:
scale = 0.25;
break;
case 3:
scale = 0.125;
break;
case 4:
scale = 0.0625;
break;
}
return (1.0f * ctrl->rr(1, BNO055P1_GYR_AM_THRES)) * scale;
}
/* ==================================================================
* 角速度センサーのAnyMotion割り込み閾値を設定する
* ------------------------------------------------------------------
* 引数threashold: AnyMotion割り込み閾値[単位:dps]
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setGyroAnyMotionThreashold(float threashold){
char r = getGyroConfig_0();
float scale = 0.0f;
switch(r & 0x07){
case 0:
scale = 1.0f;
break;
case 1:
scale = 0.5;
break;
case 2:
scale = 0.25;
break;
case 3:
scale = 0.125;
break;
case 4:
scale = 0.0625;
break;
}
return ctrl->wr(1, BNO055P1_GYR_AM_THRES, (char)((threashold / scale) + 0.5));
}
/* ==================================================================
* 加速度センサーのAnyMotion割り込み閾値を取得する
* ------------------------------------------------------------------
* returns:
* AnyMotion割り込み設定レジスタ値
*/
char BOARDC_BNO055::getAccAnyMotionSetting(){
return ctrl->rr(1, BNO055P1_GYR_AM_SET);
}
/* ==================================================================
* 加速度センサーのAnyMotion割り込み閾値を設定する
* ------------------------------------------------------------------
* 引数setting: AnyMotion割り込み設定レジスタ値
* ------------------------------------------------------------------
* returns:
* -1 失敗
* 1 成功
*/
char BOARDC_BNO055::setAccAnyMotionSetting(char setting){
return ctrl->wr(1, BNO055P1_GYR_AM_SET, setting);
}