BNO055をI2CとUARTで使用するためのライブラリ。UARTはmbedにて不安定なため使用できるボードとできないボードがある模様。
Dependents: BNO055_BME280_ Yabusame2_gyro GRhanawaizman test_deg_read
BNO055.cpp@1:7dbc931127c0, 2017-05-30 (annotated)
- Committer:
- Yajirushi
- Date:
- Tue May 30 07:07:35 2017 +0000
- Revision:
- 1:7dbc931127c0
- Parent:
- 0:3807ce385b2a
- Child:
- 3:363ec3772dce
add function (getEulerFromQ): ?????????????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yajirushi | 0:3807ce385b2a | 1 | #include "BNO055.h" |
Yajirushi | 0:3807ce385b2a | 2 | |
Yajirushi | 0:3807ce385b2a | 3 | //CLASS:BNO055_CTRL////////////////////////////////////////////////// |
Yajirushi | 0:3807ce385b2a | 4 | /* ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 5 | * BNO055_UART_CTRLクラスとBNO055_I2C_CTRLクラスの基底クラス(インターフェース) |
Yajirushi | 0:3807ce385b2a | 6 | * UARTとI2Cをヘッダ書き換えなしで実現するために無理するとこうなった |
Yajirushi | 0:3807ce385b2a | 7 | */ |
Yajirushi | 0:3807ce385b2a | 8 | |
Yajirushi | 0:3807ce385b2a | 9 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 10 | * デフォルトコンストラクタ |
Yajirushi | 0:3807ce385b2a | 11 | */ |
Yajirushi | 0:3807ce385b2a | 12 | BNO055_CTRL::BNO055_CTRL(){ |
Yajirushi | 0:3807ce385b2a | 13 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 14 | lastLength = 0; |
Yajirushi | 0:3807ce385b2a | 15 | } |
Yajirushi | 0:3807ce385b2a | 16 | |
Yajirushi | 0:3807ce385b2a | 17 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 18 | * デフォルトデストラクタ |
Yajirushi | 0:3807ce385b2a | 19 | */ |
Yajirushi | 0:3807ce385b2a | 20 | BNO055_CTRL::~BNO055_CTRL(){} |
Yajirushi | 0:3807ce385b2a | 21 | |
Yajirushi | 0:3807ce385b2a | 22 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 23 | * 現在のページIDを取得する |
Yajirushi | 0:3807ce385b2a | 24 | */ |
Yajirushi | 0:3807ce385b2a | 25 | char BNO055_CTRL::getNowPage(){ |
Yajirushi | 0:3807ce385b2a | 26 | return page1 ? 1 : 0; |
Yajirushi | 0:3807ce385b2a | 27 | } |
Yajirushi | 0:3807ce385b2a | 28 | |
Yajirushi | 0:3807ce385b2a | 29 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 30 | * UARTまたはI2Cで取得した最後のエラーを取得する:通信がうまくいかないときの原因追及 |
Yajirushi | 0:3807ce385b2a | 31 | */ |
Yajirushi | 0:3807ce385b2a | 32 | char BNO055_CTRL::getLastError(){ |
Yajirushi | 0:3807ce385b2a | 33 | return lastError; |
Yajirushi | 0:3807ce385b2a | 34 | } |
Yajirushi | 0:3807ce385b2a | 35 | |
Yajirushi | 0:3807ce385b2a | 36 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 37 | * UARTまたはI2Cで通信した際の受信バイト数を取得する:通信がうまくいかないときの原因追及 |
Yajirushi | 0:3807ce385b2a | 38 | */ |
Yajirushi | 0:3807ce385b2a | 39 | char BNO055_CTRL::getLastLength(){ |
Yajirushi | 0:3807ce385b2a | 40 | return lastLength; |
Yajirushi | 0:3807ce385b2a | 41 | } |
Yajirushi | 0:3807ce385b2a | 42 | |
Yajirushi | 0:3807ce385b2a | 43 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 44 | * 未実装関数(子クラスで実装される) |
Yajirushi | 0:3807ce385b2a | 45 | */ |
Yajirushi | 0:3807ce385b2a | 46 | void BNO055_CTRL::init(){} |
Yajirushi | 0:3807ce385b2a | 47 | char BNO055_CTRL::rr(bool isPage1, char regAddr){return 0;} |
Yajirushi | 0:3807ce385b2a | 48 | char BNO055_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){return 0;} |
Yajirushi | 0:3807ce385b2a | 49 | char BNO055_CTRL::wr(bool isPage1, char regAddr, char wBytes){return 0;} |
Yajirushi | 0:3807ce385b2a | 50 | char BNO055_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){return 0;} |
Yajirushi | 0:3807ce385b2a | 51 | |
Yajirushi | 0:3807ce385b2a | 52 | |
Yajirushi | 0:3807ce385b2a | 53 | |
Yajirushi | 0:3807ce385b2a | 54 | |
Yajirushi | 0:3807ce385b2a | 55 | |
Yajirushi | 0:3807ce385b2a | 56 | |
Yajirushi | 0:3807ce385b2a | 57 | |
Yajirushi | 0:3807ce385b2a | 58 | |
Yajirushi | 0:3807ce385b2a | 59 | |
Yajirushi | 0:3807ce385b2a | 60 | |
Yajirushi | 0:3807ce385b2a | 61 | //CLASS:BNO055_UART_CTRL///////////////////////////////////////////// |
Yajirushi | 0:3807ce385b2a | 62 | /* ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 63 | * BNO055_CTRLクラス(インターフェース)を継承(実装)したクラス |
Yajirushi | 0:3807ce385b2a | 64 | * UARTで命令を送受信するためのコントロール用クラス |
Yajirushi | 0:3807ce385b2a | 65 | */ |
Yajirushi | 0:3807ce385b2a | 66 | |
Yajirushi | 0:3807ce385b2a | 67 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 68 | * BNO055をUARTでコントロールするためのクラス:コンストラクタ |
Yajirushi | 0:3807ce385b2a | 69 | */ |
Yajirushi | 0:3807ce385b2a | 70 | BNO055_UART_CTRL::BNO055_UART_CTRL(RawSerial *uart){ |
Yajirushi | 0:3807ce385b2a | 71 | iface = uart; |
Yajirushi | 0:3807ce385b2a | 72 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 73 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 74 | |
Yajirushi | 0:3807ce385b2a | 75 | page1 = true; |
Yajirushi | 0:3807ce385b2a | 76 | |
Yajirushi | 0:3807ce385b2a | 77 | ary = new char[BNO055_UART_BUF_MAXLEN + 5]; |
Yajirushi | 0:3807ce385b2a | 78 | memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); |
Yajirushi | 0:3807ce385b2a | 79 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 80 | } |
Yajirushi | 0:3807ce385b2a | 81 | |
Yajirushi | 0:3807ce385b2a | 82 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 83 | * BNO055をUARTでコントロールするためのクラス:デストラクタ |
Yajirushi | 0:3807ce385b2a | 84 | */ |
Yajirushi | 0:3807ce385b2a | 85 | BNO055_UART_CTRL::~BNO055_UART_CTRL(){ |
Yajirushi | 0:3807ce385b2a | 86 | delete iface; |
Yajirushi | 0:3807ce385b2a | 87 | } |
Yajirushi | 0:3807ce385b2a | 88 | |
Yajirushi | 0:3807ce385b2a | 89 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 90 | * UART受信割り込み用関数 |
Yajirushi | 0:3807ce385b2a | 91 | * RX受信トリガがONになると、カウンタを加算する |
Yajirushi | 0:3807ce385b2a | 92 | */ |
Yajirushi | 0:3807ce385b2a | 93 | void BNO055_UART_CTRL::rxInterrupt(){ |
Yajirushi | 0:3807ce385b2a | 94 | if(read_mark){ |
Yajirushi | 0:3807ce385b2a | 95 | rxd = iface->getc(); |
Yajirushi | 0:3807ce385b2a | 96 | read_mark = false; |
Yajirushi | 0:3807ce385b2a | 97 | } |
Yajirushi | 0:3807ce385b2a | 98 | } |
Yajirushi | 0:3807ce385b2a | 99 | |
Yajirushi | 0:3807ce385b2a | 100 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 101 | * <UART> |
Yajirushi | 0:3807ce385b2a | 102 | * レジスタの内容を読み取り(1byteのみ) |
Yajirushi | 0:3807ce385b2a | 103 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 104 | * returns: |
Yajirushi | 0:3807ce385b2a | 105 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 106 | * それ以外 成功した際に取得したデータ |
Yajirushi | 0:3807ce385b2a | 107 | */ |
Yajirushi | 0:3807ce385b2a | 108 | char BNO055_UART_CTRL::rr(bool isPage1, char regAddr){ |
Yajirushi | 0:3807ce385b2a | 109 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 110 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 111 | wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0); |
Yajirushi | 0:3807ce385b2a | 112 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 113 | } |
Yajirushi | 0:3807ce385b2a | 114 | |
Yajirushi | 0:3807ce385b2a | 115 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 116 | do{wait_ms(1);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 117 | |
Yajirushi | 0:3807ce385b2a | 118 | //コマンドをセット |
Yajirushi | 0:3807ce385b2a | 119 | ary[0] = 0xAA; //StartByte(固定) |
Yajirushi | 0:3807ce385b2a | 120 | ary[1] = 0x01; //読み取り |
Yajirushi | 0:3807ce385b2a | 121 | ary[2] = regAddr; //レジスタアドレス |
Yajirushi | 0:3807ce385b2a | 122 | ary[3] = 1; //バイト長 |
Yajirushi | 0:3807ce385b2a | 123 | |
Yajirushi | 0:3807ce385b2a | 124 | //送信 |
Yajirushi | 0:3807ce385b2a | 125 | iface->putc(ary[0]); |
Yajirushi | 0:3807ce385b2a | 126 | iface->putc(ary[1]); |
Yajirushi | 0:3807ce385b2a | 127 | iface->putc(ary[2]); |
Yajirushi | 0:3807ce385b2a | 128 | iface->putc(ary[3]); |
Yajirushi | 0:3807ce385b2a | 129 | |
Yajirushi | 0:3807ce385b2a | 130 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 131 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 132 | |
Yajirushi | 0:3807ce385b2a | 133 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 134 | memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア |
Yajirushi | 0:3807ce385b2a | 135 | |
Yajirushi | 0:3807ce385b2a | 136 | ary[0] = rxd & 0xFF; |
Yajirushi | 0:3807ce385b2a | 137 | |
Yajirushi | 0:3807ce385b2a | 138 | //残りを受信 |
Yajirushi | 0:3807ce385b2a | 139 | int i = 1; |
Yajirushi | 0:3807ce385b2a | 140 | int cnt = (ary[0] == 0xBB) ? 2 : 1; |
Yajirushi | 0:3807ce385b2a | 141 | while(i < cnt && iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 142 | ary[i++] = iface->getc(); |
Yajirushi | 0:3807ce385b2a | 143 | } |
Yajirushi | 0:3807ce385b2a | 144 | |
Yajirushi | 0:3807ce385b2a | 145 | //レスポンスが0xBB以外:通信失敗 |
Yajirushi | 0:3807ce385b2a | 146 | if(ary[0] != 0xBB){ |
Yajirushi | 0:3807ce385b2a | 147 | lastLength = 2; |
Yajirushi | 0:3807ce385b2a | 148 | lastError = ary[1]; |
Yajirushi | 0:3807ce385b2a | 149 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 150 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 151 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 152 | return -1; |
Yajirushi | 0:3807ce385b2a | 153 | } |
Yajirushi | 0:3807ce385b2a | 154 | |
Yajirushi | 0:3807ce385b2a | 155 | lastLength = ary[1] + 2; |
Yajirushi | 0:3807ce385b2a | 156 | |
Yajirushi | 0:3807ce385b2a | 157 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 158 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 159 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 160 | |
Yajirushi | 0:3807ce385b2a | 161 | //通信成功時、取得データを返す |
Yajirushi | 0:3807ce385b2a | 162 | return ary[2]; |
Yajirushi | 0:3807ce385b2a | 163 | } |
Yajirushi | 0:3807ce385b2a | 164 | |
Yajirushi | 0:3807ce385b2a | 165 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 166 | * <UART> |
Yajirushi | 0:3807ce385b2a | 167 | * レジスタの内容を読み取り(複数可) |
Yajirushi | 0:3807ce385b2a | 168 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 169 | * returns: |
Yajirushi | 0:3807ce385b2a | 170 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 171 | * -2 返答バイト不一致 |
Yajirushi | 0:3807ce385b2a | 172 | * -4 レスポンスエラー |
Yajirushi | 0:3807ce385b2a | 173 | * それ以外 成功した際の戻り値バイト数 |
Yajirushi | 0:3807ce385b2a | 174 | */ |
Yajirushi | 0:3807ce385b2a | 175 | char BNO055_UART_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){ |
Yajirushi | 0:3807ce385b2a | 176 | //読み取りバイト数が1未満またはBNO055_UART_BUF_MAXLEN以上はバッファが足りないので読み取れない |
Yajirushi | 0:3807ce385b2a | 177 | if(length < 1 || length > BNO055_UART_BUF_MAXLEN) return -1; |
Yajirushi | 0:3807ce385b2a | 178 | |
Yajirushi | 0:3807ce385b2a | 179 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 180 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 181 | wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0); |
Yajirushi | 0:3807ce385b2a | 182 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 183 | } |
Yajirushi | 0:3807ce385b2a | 184 | |
Yajirushi | 0:3807ce385b2a | 185 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 186 | do{wait_ms(1);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 187 | |
Yajirushi | 0:3807ce385b2a | 188 | //コマンドをセット |
Yajirushi | 0:3807ce385b2a | 189 | ary[0] = 0xAA; //StartByte(固定) |
Yajirushi | 0:3807ce385b2a | 190 | ary[1] = 0x01; //読み取り |
Yajirushi | 0:3807ce385b2a | 191 | ary[2] = startRegAddr; //読み取り開始レジスタアドレス |
Yajirushi | 0:3807ce385b2a | 192 | ary[3] = length; //バイト長 |
Yajirushi | 0:3807ce385b2a | 193 | |
Yajirushi | 0:3807ce385b2a | 194 | //送信 |
Yajirushi | 0:3807ce385b2a | 195 | iface->putc(ary[0]); |
Yajirushi | 0:3807ce385b2a | 196 | iface->putc(ary[1]); |
Yajirushi | 0:3807ce385b2a | 197 | iface->putc(ary[2]); |
Yajirushi | 0:3807ce385b2a | 198 | iface->putc(ary[3]); |
Yajirushi | 0:3807ce385b2a | 199 | |
Yajirushi | 0:3807ce385b2a | 200 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 201 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 202 | |
Yajirushi | 0:3807ce385b2a | 203 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 204 | memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア |
Yajirushi | 0:3807ce385b2a | 205 | |
Yajirushi | 0:3807ce385b2a | 206 | ary[0] = rxd & 0xFF; |
Yajirushi | 0:3807ce385b2a | 207 | |
Yajirushi | 0:3807ce385b2a | 208 | //残りを受信 |
Yajirushi | 0:3807ce385b2a | 209 | int i = 1; |
Yajirushi | 0:3807ce385b2a | 210 | int cnt = (ary[0] == 0xBB) ? 2 : 1; |
Yajirushi | 0:3807ce385b2a | 211 | while(i < cnt && iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 212 | ary[i++] = iface->getc(); |
Yajirushi | 0:3807ce385b2a | 213 | } |
Yajirushi | 0:3807ce385b2a | 214 | |
Yajirushi | 0:3807ce385b2a | 215 | //レスポンスが0xBB以外:通信失敗 |
Yajirushi | 0:3807ce385b2a | 216 | if(ary[0] != 0xBB){ |
Yajirushi | 0:3807ce385b2a | 217 | lastLength = 2; |
Yajirushi | 0:3807ce385b2a | 218 | lastError = ary[1]; |
Yajirushi | 0:3807ce385b2a | 219 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 220 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 221 | return -1; |
Yajirushi | 0:3807ce385b2a | 222 | } |
Yajirushi | 0:3807ce385b2a | 223 | |
Yajirushi | 0:3807ce385b2a | 224 | //返答バイト長がlengthと一致しない:通信失敗 |
Yajirushi | 0:3807ce385b2a | 225 | if(ary[1] != length){ |
Yajirushi | 0:3807ce385b2a | 226 | lastLength = ary[1]; |
Yajirushi | 0:3807ce385b2a | 227 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 228 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 229 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 230 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 231 | return -2; |
Yajirushi | 0:3807ce385b2a | 232 | } |
Yajirushi | 0:3807ce385b2a | 233 | |
Yajirushi | 0:3807ce385b2a | 234 | lastLength = ary[1] + 2; |
Yajirushi | 0:3807ce385b2a | 235 | |
Yajirushi | 0:3807ce385b2a | 236 | memcpy(receiveBytes, ary+2, ary[1]); |
Yajirushi | 0:3807ce385b2a | 237 | |
Yajirushi | 0:3807ce385b2a | 238 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 239 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 240 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 241 | |
Yajirushi | 0:3807ce385b2a | 242 | return ary[1]; |
Yajirushi | 0:3807ce385b2a | 243 | } |
Yajirushi | 0:3807ce385b2a | 244 | |
Yajirushi | 0:3807ce385b2a | 245 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 246 | * <UART> |
Yajirushi | 0:3807ce385b2a | 247 | * レジスタ書き込み(1byteのみ) |
Yajirushi | 0:3807ce385b2a | 248 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 249 | * returns: |
Yajirushi | 0:3807ce385b2a | 250 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 251 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 252 | */ |
Yajirushi | 0:3807ce385b2a | 253 | char BNO055_UART_CTRL::wr(bool isPage1, char regAddr, char wBytes){ |
Yajirushi | 0:3807ce385b2a | 254 | //ページが異なるならページ変更命令を発行(再帰処理) |
Yajirushi | 0:3807ce385b2a | 255 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 256 | wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0); |
Yajirushi | 0:3807ce385b2a | 257 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 258 | } |
Yajirushi | 0:3807ce385b2a | 259 | |
Yajirushi | 0:3807ce385b2a | 260 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 261 | do{wait_ms(1);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 262 | |
Yajirushi | 0:3807ce385b2a | 263 | //コマンドをセット |
Yajirushi | 0:3807ce385b2a | 264 | ary[0] = 0xAA; //StartByte(固定) |
Yajirushi | 0:3807ce385b2a | 265 | ary[1] = 0x00; //書き込み |
Yajirushi | 0:3807ce385b2a | 266 | ary[2] = regAddr; //レジスタアドレス |
Yajirushi | 0:3807ce385b2a | 267 | ary[3] = 1; //バイト長 |
Yajirushi | 0:3807ce385b2a | 268 | ary[4] = wBytes; //送信データ |
Yajirushi | 0:3807ce385b2a | 269 | |
Yajirushi | 0:3807ce385b2a | 270 | //送信 |
Yajirushi | 0:3807ce385b2a | 271 | iface->putc(ary[0]); |
Yajirushi | 0:3807ce385b2a | 272 | iface->putc(ary[1]); |
Yajirushi | 0:3807ce385b2a | 273 | iface->putc(ary[2]); |
Yajirushi | 0:3807ce385b2a | 274 | iface->putc(ary[3]); |
Yajirushi | 0:3807ce385b2a | 275 | iface->putc(ary[4]); |
Yajirushi | 0:3807ce385b2a | 276 | |
Yajirushi | 0:3807ce385b2a | 277 | //システムリブートが発生するレジスタの場合は1200ms待つ |
Yajirushi | 0:3807ce385b2a | 278 | if(regAddr == 0x3F) wait_ms(1200); |
Yajirushi | 0:3807ce385b2a | 279 | |
Yajirushi | 0:3807ce385b2a | 280 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 281 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 282 | |
Yajirushi | 0:3807ce385b2a | 283 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 284 | memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア |
Yajirushi | 0:3807ce385b2a | 285 | |
Yajirushi | 0:3807ce385b2a | 286 | ary[0] = rxd & 0xFF; |
Yajirushi | 0:3807ce385b2a | 287 | |
Yajirushi | 0:3807ce385b2a | 288 | //残りを受信 |
Yajirushi | 0:3807ce385b2a | 289 | while(iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 290 | ary[1] = iface->getc(); |
Yajirushi | 0:3807ce385b2a | 291 | } |
Yajirushi | 0:3807ce385b2a | 292 | |
Yajirushi | 0:3807ce385b2a | 293 | //レスポンスが0xEE以外もしくはステータスが0x01以外:書き込み失敗 |
Yajirushi | 0:3807ce385b2a | 294 | if(ary[0] != 0xEE || ary[1] != 0x01){ |
Yajirushi | 0:3807ce385b2a | 295 | lastLength = 2; |
Yajirushi | 0:3807ce385b2a | 296 | lastError = ary[1]; |
Yajirushi | 0:3807ce385b2a | 297 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 298 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 299 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 300 | return -1; |
Yajirushi | 0:3807ce385b2a | 301 | } |
Yajirushi | 0:3807ce385b2a | 302 | |
Yajirushi | 0:3807ce385b2a | 303 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 304 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 305 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 306 | |
Yajirushi | 0:3807ce385b2a | 307 | return 1; |
Yajirushi | 0:3807ce385b2a | 308 | } |
Yajirushi | 0:3807ce385b2a | 309 | |
Yajirushi | 0:3807ce385b2a | 310 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 311 | * <UART> |
Yajirushi | 0:3807ce385b2a | 312 | * レジスタ書き込み(複数可) |
Yajirushi | 0:3807ce385b2a | 313 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 314 | * returns: |
Yajirushi | 0:3807ce385b2a | 315 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 316 | * -4 レスポンスエラー |
Yajirushi | 0:3807ce385b2a | 317 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 318 | */ |
Yajirushi | 0:3807ce385b2a | 319 | char BNO055_UART_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){ |
Yajirushi | 0:3807ce385b2a | 320 | //書き込みバイト数が1未満またはBNO055_UART_BUF_MAXLEN以上はバッファが足りないので読み取れない |
Yajirushi | 0:3807ce385b2a | 321 | if(length < 1 || length > BNO055_UART_BUF_MAXLEN) return -1; |
Yajirushi | 0:3807ce385b2a | 322 | |
Yajirushi | 0:3807ce385b2a | 323 | //ページが異なるならページ変更命令を発行(再帰処理) |
Yajirushi | 0:3807ce385b2a | 324 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 325 | wr(page1, BNO055_PAGE_ID, (isPage1) ? 1 : 0); |
Yajirushi | 0:3807ce385b2a | 326 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 327 | } |
Yajirushi | 0:3807ce385b2a | 328 | |
Yajirushi | 0:3807ce385b2a | 329 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 330 | do{wait_ms(1);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 331 | |
Yajirushi | 0:3807ce385b2a | 332 | //コマンドをセット |
Yajirushi | 0:3807ce385b2a | 333 | ary[0] = 0xAA; //StartByte(固定) |
Yajirushi | 0:3807ce385b2a | 334 | ary[1] = 0x00; //書き込み |
Yajirushi | 0:3807ce385b2a | 335 | ary[2] = startRegAddr; //レジスタアドレス |
Yajirushi | 0:3807ce385b2a | 336 | ary[3] = length; //バイト長 |
Yajirushi | 0:3807ce385b2a | 337 | |
Yajirushi | 0:3807ce385b2a | 338 | //前部分送信 |
Yajirushi | 0:3807ce385b2a | 339 | iface->putc(ary[0]); |
Yajirushi | 0:3807ce385b2a | 340 | iface->putc(ary[1]); |
Yajirushi | 0:3807ce385b2a | 341 | iface->putc(ary[2]); |
Yajirushi | 0:3807ce385b2a | 342 | iface->putc(ary[3]); |
Yajirushi | 0:3807ce385b2a | 343 | |
Yajirushi | 0:3807ce385b2a | 344 | //データ内容送信 |
Yajirushi | 0:3807ce385b2a | 345 | for(int cnt=0; cnt<length; cnt++){ |
Yajirushi | 0:3807ce385b2a | 346 | iface->putc(Bytes[cnt]); |
Yajirushi | 0:3807ce385b2a | 347 | } |
Yajirushi | 0:3807ce385b2a | 348 | |
Yajirushi | 0:3807ce385b2a | 349 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 350 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 351 | |
Yajirushi | 0:3807ce385b2a | 352 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 353 | memset(ary, 0, BNO055_UART_BUF_MAXLEN + 5); //配列ゼロクリア |
Yajirushi | 0:3807ce385b2a | 354 | |
Yajirushi | 0:3807ce385b2a | 355 | ary[0] = rxd & 0xFF; |
Yajirushi | 0:3807ce385b2a | 356 | |
Yajirushi | 0:3807ce385b2a | 357 | //残りを受信 |
Yajirushi | 0:3807ce385b2a | 358 | while(iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 359 | ary[1] = iface->getc(); |
Yajirushi | 0:3807ce385b2a | 360 | } |
Yajirushi | 0:3807ce385b2a | 361 | |
Yajirushi | 0:3807ce385b2a | 362 | //レスポンスが0xEE以外もしくはステータスが0x01以外:書き込み失敗 |
Yajirushi | 0:3807ce385b2a | 363 | if(ary[0] != 0xEE || ary[1] != 0x01){ |
Yajirushi | 0:3807ce385b2a | 364 | lastLength = 2; |
Yajirushi | 0:3807ce385b2a | 365 | lastError = ary[1]; |
Yajirushi | 0:3807ce385b2a | 366 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 367 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 368 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 369 | return -1; |
Yajirushi | 0:3807ce385b2a | 370 | } |
Yajirushi | 0:3807ce385b2a | 371 | |
Yajirushi | 0:3807ce385b2a | 372 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 373 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 374 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 375 | |
Yajirushi | 0:3807ce385b2a | 376 | return 1; |
Yajirushi | 0:3807ce385b2a | 377 | } |
Yajirushi | 0:3807ce385b2a | 378 | |
Yajirushi | 0:3807ce385b2a | 379 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 380 | * <UART> |
Yajirushi | 0:3807ce385b2a | 381 | * インターフェース設定を初期化する |
Yajirushi | 0:3807ce385b2a | 382 | */ |
Yajirushi | 0:3807ce385b2a | 383 | void BNO055_UART_CTRL::init(){ |
Yajirushi | 0:3807ce385b2a | 384 | iface->format(); //8N1 = default |
Yajirushi | 0:3807ce385b2a | 385 | iface->baud(115200); |
Yajirushi | 0:3807ce385b2a | 386 | wait_ms(5); |
Yajirushi | 0:3807ce385b2a | 387 | iface->attach(this, &BNO055_UART_CTRL::rxInterrupt); |
Yajirushi | 0:3807ce385b2a | 388 | |
Yajirushi | 0:3807ce385b2a | 389 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 390 | do{wait_ms(1);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 391 | |
Yajirushi | 0:3807ce385b2a | 392 | //UARTリセット時に0xFFがつく現象および前回送信時の残りがあるため同期がとれない |
Yajirushi | 0:3807ce385b2a | 393 | //複数回読み取りを行い、正しいレスポンス(0xBB)が返るまで送信 |
Yajirushi | 0:3807ce385b2a | 394 | iface->putc(0x01); //dummy |
Yajirushi | 0:3807ce385b2a | 395 | iface->putc(0x01); //dummy |
Yajirushi | 0:3807ce385b2a | 396 | iface->putc(0x01); //dummy |
Yajirushi | 0:3807ce385b2a | 397 | iface->putc(0x01); //dummy |
Yajirushi | 0:3807ce385b2a | 398 | |
Yajirushi | 0:3807ce385b2a | 399 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 400 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 401 | |
Yajirushi | 0:3807ce385b2a | 402 | char rslt = rxd & 0xFF; //レスポンス |
Yajirushi | 0:3807ce385b2a | 403 | |
Yajirushi | 0:3807ce385b2a | 404 | do{ |
Yajirushi | 0:3807ce385b2a | 405 | //レスポンスが異常:残りの情報を無視 |
Yajirushi | 0:3807ce385b2a | 406 | if(rslt != 0xBB){ |
Yajirushi | 0:3807ce385b2a | 407 | while(iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 408 | iface->getc(); |
Yajirushi | 0:3807ce385b2a | 409 | } |
Yajirushi | 0:3807ce385b2a | 410 | } |
Yajirushi | 0:3807ce385b2a | 411 | |
Yajirushi | 0:3807ce385b2a | 412 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 413 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 414 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 415 | |
Yajirushi | 0:3807ce385b2a | 416 | //送信可能になるまで待つ |
Yajirushi | 0:3807ce385b2a | 417 | do{wait_ms(5);}while(!iface->writeable()); |
Yajirushi | 0:3807ce385b2a | 418 | |
Yajirushi | 0:3807ce385b2a | 419 | iface->putc(0xAA); //START |
Yajirushi | 0:3807ce385b2a | 420 | iface->putc(0x01); //読み取り |
Yajirushi | 0:3807ce385b2a | 421 | iface->putc(0x07); //ページID |
Yajirushi | 0:3807ce385b2a | 422 | iface->putc(0x01); //length 1byte |
Yajirushi | 0:3807ce385b2a | 423 | |
Yajirushi | 0:3807ce385b2a | 424 | //レスポンスを受信するまで待つ |
Yajirushi | 0:3807ce385b2a | 425 | while(read_mark) wait_us(100); |
Yajirushi | 0:3807ce385b2a | 426 | |
Yajirushi | 0:3807ce385b2a | 427 | rslt = rxd & 0xFF; //レスポンス |
Yajirushi | 0:3807ce385b2a | 428 | |
Yajirushi | 0:3807ce385b2a | 429 | }while(rslt != 0xBB); |
Yajirushi | 0:3807ce385b2a | 430 | |
Yajirushi | 0:3807ce385b2a | 431 | //残りの情報を無視 |
Yajirushi | 0:3807ce385b2a | 432 | while(iface->readable()){ |
Yajirushi | 0:3807ce385b2a | 433 | iface->getc(); |
Yajirushi | 0:3807ce385b2a | 434 | } |
Yajirushi | 0:3807ce385b2a | 435 | |
Yajirushi | 0:3807ce385b2a | 436 | //受信用割り込みマークをリセット |
Yajirushi | 0:3807ce385b2a | 437 | rxd = 0xFFFF; |
Yajirushi | 0:3807ce385b2a | 438 | read_mark = true; |
Yajirushi | 0:3807ce385b2a | 439 | } |
Yajirushi | 0:3807ce385b2a | 440 | |
Yajirushi | 0:3807ce385b2a | 441 | |
Yajirushi | 0:3807ce385b2a | 442 | |
Yajirushi | 0:3807ce385b2a | 443 | |
Yajirushi | 0:3807ce385b2a | 444 | |
Yajirushi | 0:3807ce385b2a | 445 | |
Yajirushi | 0:3807ce385b2a | 446 | |
Yajirushi | 0:3807ce385b2a | 447 | |
Yajirushi | 0:3807ce385b2a | 448 | //CLASS:BNO055_I2C_CTRL////////////////////////////////////////////// |
Yajirushi | 0:3807ce385b2a | 449 | /* ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 450 | * BNO055_CTRLクラス(インターフェース)を継承(実装)したクラス |
Yajirushi | 0:3807ce385b2a | 451 | * I2Cで命令を送受信するためのコントロール用クラス |
Yajirushi | 0:3807ce385b2a | 452 | */ |
Yajirushi | 0:3807ce385b2a | 453 | |
Yajirushi | 0:3807ce385b2a | 454 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 455 | * BNO055をI2Cでコントロールするためのクラス:コンストラクタ |
Yajirushi | 0:3807ce385b2a | 456 | */ |
Yajirushi | 0:3807ce385b2a | 457 | BNO055_I2C_CTRL::BNO055_I2C_CTRL(I2C* iic, char addr, unsigned int freq){ |
Yajirushi | 0:3807ce385b2a | 458 | iface = iic; |
Yajirushi | 0:3807ce385b2a | 459 | i2c_writeAddr = addr << 1; |
Yajirushi | 0:3807ce385b2a | 460 | i2c_readAddr = i2c_writeAddr + 1; |
Yajirushi | 0:3807ce385b2a | 461 | i2c_freq = freq; |
Yajirushi | 0:3807ce385b2a | 462 | page1 = true; |
Yajirushi | 0:3807ce385b2a | 463 | ary = new char[2]; |
Yajirushi | 0:3807ce385b2a | 464 | memset(ary, 0, 2); |
Yajirushi | 0:3807ce385b2a | 465 | lastError = 0; |
Yajirushi | 0:3807ce385b2a | 466 | } |
Yajirushi | 0:3807ce385b2a | 467 | |
Yajirushi | 0:3807ce385b2a | 468 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 469 | * BNO055をI2Cでコントロールするためのクラス:デストラクタ |
Yajirushi | 0:3807ce385b2a | 470 | */ |
Yajirushi | 0:3807ce385b2a | 471 | BNO055_I2C_CTRL::~BNO055_I2C_CTRL(){ |
Yajirushi | 0:3807ce385b2a | 472 | delete iface; |
Yajirushi | 0:3807ce385b2a | 473 | delete[] ary; |
Yajirushi | 0:3807ce385b2a | 474 | } |
Yajirushi | 0:3807ce385b2a | 475 | |
Yajirushi | 0:3807ce385b2a | 476 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 477 | * <I2C> |
Yajirushi | 0:3807ce385b2a | 478 | * レジスタの内容を読み取り(1byteのみ) |
Yajirushi | 0:3807ce385b2a | 479 | */ |
Yajirushi | 0:3807ce385b2a | 480 | char BNO055_I2C_CTRL::rr(bool isPage1, char regAddr){ |
Yajirushi | 0:3807ce385b2a | 481 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 482 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 483 | ary[0] = BNO055_PAGE_ID; |
Yajirushi | 0:3807ce385b2a | 484 | ary[1] = (isPage1) ? 1 : 0; |
Yajirushi | 0:3807ce385b2a | 485 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 486 | iface->write(i2c_writeAddr, ary, 2); |
Yajirushi | 0:3807ce385b2a | 487 | } |
Yajirushi | 0:3807ce385b2a | 488 | |
Yajirushi | 0:3807ce385b2a | 489 | ary[0] = regAddr; |
Yajirushi | 0:3807ce385b2a | 490 | iface->write(i2c_writeAddr, ary, 1, true); |
Yajirushi | 0:3807ce385b2a | 491 | iface->read(i2c_readAddr, ary, 1, false); |
Yajirushi | 0:3807ce385b2a | 492 | |
Yajirushi | 0:3807ce385b2a | 493 | return ary[0]; |
Yajirushi | 0:3807ce385b2a | 494 | } |
Yajirushi | 0:3807ce385b2a | 495 | |
Yajirushi | 0:3807ce385b2a | 496 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 497 | * <I2C> |
Yajirushi | 0:3807ce385b2a | 498 | * レジスタの内容を読み取り(複数可) |
Yajirushi | 0:3807ce385b2a | 499 | */ |
Yajirushi | 0:3807ce385b2a | 500 | char BNO055_I2C_CTRL::rrc(bool isPage1, char startRegAddr, char *receiveBytes, char length){ |
Yajirushi | 0:3807ce385b2a | 501 | //読み取りバイト数が1未満 |
Yajirushi | 0:3807ce385b2a | 502 | if(length < 1) return -1; |
Yajirushi | 0:3807ce385b2a | 503 | |
Yajirushi | 0:3807ce385b2a | 504 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 505 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 506 | ary[0] = BNO055_PAGE_ID; |
Yajirushi | 0:3807ce385b2a | 507 | ary[1] = (isPage1) ? 1 : 0; |
Yajirushi | 0:3807ce385b2a | 508 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 509 | iface->write(i2c_writeAddr, ary, 2); |
Yajirushi | 0:3807ce385b2a | 510 | } |
Yajirushi | 0:3807ce385b2a | 511 | |
Yajirushi | 0:3807ce385b2a | 512 | ary[0] = startRegAddr; |
Yajirushi | 0:3807ce385b2a | 513 | iface->write(i2c_writeAddr, ary, 1, true); |
Yajirushi | 0:3807ce385b2a | 514 | iface->read(i2c_readAddr, receiveBytes, length, false); |
Yajirushi | 0:3807ce385b2a | 515 | |
Yajirushi | 0:3807ce385b2a | 516 | return receiveBytes[0]; |
Yajirushi | 0:3807ce385b2a | 517 | } |
Yajirushi | 0:3807ce385b2a | 518 | |
Yajirushi | 0:3807ce385b2a | 519 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 520 | * <I2C> |
Yajirushi | 0:3807ce385b2a | 521 | * レジスタ書き込み(1byteのみ) |
Yajirushi | 0:3807ce385b2a | 522 | */ |
Yajirushi | 0:3807ce385b2a | 523 | char BNO055_I2C_CTRL::wr(bool isPage1, char regAddr, char wBytes){ |
Yajirushi | 0:3807ce385b2a | 524 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 525 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 526 | ary[0] = BNO055_PAGE_ID; |
Yajirushi | 0:3807ce385b2a | 527 | ary[1] = (isPage1) ? 1 : 0; |
Yajirushi | 0:3807ce385b2a | 528 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 529 | iface->write(i2c_writeAddr, ary, 2); |
Yajirushi | 0:3807ce385b2a | 530 | } |
Yajirushi | 0:3807ce385b2a | 531 | |
Yajirushi | 0:3807ce385b2a | 532 | ary[0] = regAddr; |
Yajirushi | 0:3807ce385b2a | 533 | ary[1] = wBytes; |
Yajirushi | 0:3807ce385b2a | 534 | |
Yajirushi | 0:3807ce385b2a | 535 | iface->write(i2c_writeAddr, ary, 2); |
Yajirushi | 0:3807ce385b2a | 536 | |
Yajirushi | 0:3807ce385b2a | 537 | return ary[0]; |
Yajirushi | 0:3807ce385b2a | 538 | } |
Yajirushi | 0:3807ce385b2a | 539 | |
Yajirushi | 0:3807ce385b2a | 540 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 541 | * <I2C> |
Yajirushi | 0:3807ce385b2a | 542 | * レジスタ書き込み(複数可) |
Yajirushi | 0:3807ce385b2a | 543 | */ |
Yajirushi | 0:3807ce385b2a | 544 | char BNO055_I2C_CTRL::wrc(bool isPage1, char startRegAddr, char *Bytes, char length){ |
Yajirushi | 0:3807ce385b2a | 545 | //書き込みバイト数が1未満 |
Yajirushi | 0:3807ce385b2a | 546 | if(length < 1) return -1; |
Yajirushi | 0:3807ce385b2a | 547 | |
Yajirushi | 0:3807ce385b2a | 548 | //ページが異なるならページ変更命令を発行 |
Yajirushi | 0:3807ce385b2a | 549 | if(page1 != isPage1){ |
Yajirushi | 0:3807ce385b2a | 550 | ary[0] = BNO055_PAGE_ID; |
Yajirushi | 0:3807ce385b2a | 551 | ary[1] = (isPage1) ? 1 : 0; |
Yajirushi | 0:3807ce385b2a | 552 | page1 = isPage1; |
Yajirushi | 0:3807ce385b2a | 553 | iface->write(i2c_writeAddr, ary, 2); |
Yajirushi | 0:3807ce385b2a | 554 | } |
Yajirushi | 0:3807ce385b2a | 555 | |
Yajirushi | 0:3807ce385b2a | 556 | ary[0] = startRegAddr; |
Yajirushi | 0:3807ce385b2a | 557 | iface->write(i2c_writeAddr, ary, 1, true); |
Yajirushi | 0:3807ce385b2a | 558 | iface->write(i2c_writeAddr, Bytes, length, false); |
Yajirushi | 0:3807ce385b2a | 559 | |
Yajirushi | 0:3807ce385b2a | 560 | return Bytes[0]; |
Yajirushi | 0:3807ce385b2a | 561 | } |
Yajirushi | 0:3807ce385b2a | 562 | |
Yajirushi | 0:3807ce385b2a | 563 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 564 | * <I2C> |
Yajirushi | 0:3807ce385b2a | 565 | * インターフェース設定を初期化する |
Yajirushi | 0:3807ce385b2a | 566 | */ |
Yajirushi | 0:3807ce385b2a | 567 | void BNO055_I2C_CTRL::init(){ |
Yajirushi | 0:3807ce385b2a | 568 | iface->frequency(i2c_freq); |
Yajirushi | 0:3807ce385b2a | 569 | wait_ms(5); |
Yajirushi | 0:3807ce385b2a | 570 | } |
Yajirushi | 0:3807ce385b2a | 571 | |
Yajirushi | 0:3807ce385b2a | 572 | |
Yajirushi | 0:3807ce385b2a | 573 | |
Yajirushi | 0:3807ce385b2a | 574 | |
Yajirushi | 0:3807ce385b2a | 575 | |
Yajirushi | 0:3807ce385b2a | 576 | |
Yajirushi | 0:3807ce385b2a | 577 | |
Yajirushi | 0:3807ce385b2a | 578 | |
Yajirushi | 0:3807ce385b2a | 579 | |
Yajirushi | 0:3807ce385b2a | 580 | //CLASS:BOARDC_BNO055//////////////////////////////////////////////// |
Yajirushi | 0:3807ce385b2a | 581 | /* ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 582 | * メインクラス |
Yajirushi | 0:3807ce385b2a | 583 | * BNO055をUARTまたはI2Cで使用するための命令をまとめている |
Yajirushi | 0:3807ce385b2a | 584 | * 内部にBNO055_CTRLクラスのインスタンスを持ち、コンストラクタの引数によって |
Yajirushi | 0:3807ce385b2a | 585 | * UARTとI2Cのどちらを使用するか決定する |
Yajirushi | 0:3807ce385b2a | 586 | */ |
Yajirushi | 0:3807ce385b2a | 587 | |
Yajirushi | 0:3807ce385b2a | 588 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 589 | * コンストラクタ (オーバーロード+3) |
Yajirushi | 0:3807ce385b2a | 590 | * UARTで使用する際のコンストラクタ:ピン別名を指定する |
Yajirushi | 0:3807ce385b2a | 591 | */ |
Yajirushi | 0:3807ce385b2a | 592 | BOARDC_BNO055::BOARDC_BNO055(PinName tx, PinName rx){ |
Yajirushi | 0:3807ce385b2a | 593 | ctrl = new BNO055_UART_CTRL(new RawSerial(tx, rx)); |
Yajirushi | 0:3807ce385b2a | 594 | |
Yajirushi | 0:3807ce385b2a | 595 | scaleACC = 0.01f; // = 1 / 100 |
Yajirushi | 0:3807ce385b2a | 596 | scaleMAG = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 597 | scaleGYRO = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 598 | scaleEuler = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 599 | scaleTEMP = 1.0f; |
Yajirushi | 0:3807ce385b2a | 600 | scaleLIA = scaleACC; |
Yajirushi | 0:3807ce385b2a | 601 | scaleGV = scaleACC; |
Yajirushi | 0:3807ce385b2a | 602 | scaleQuaternion = 0.00006103515; // = 1 / (2^14) |
Yajirushi | 0:3807ce385b2a | 603 | |
Yajirushi | 0:3807ce385b2a | 604 | clkExt = false; |
Yajirushi | 0:3807ce385b2a | 605 | } |
Yajirushi | 0:3807ce385b2a | 606 | |
Yajirushi | 0:3807ce385b2a | 607 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 608 | * コンストラクタ (オーバーロード+3) |
Yajirushi | 0:3807ce385b2a | 609 | * UARTで使用する際のコンストラクタ:RawSerialクラスのインスタンスを指定する |
Yajirushi | 0:3807ce385b2a | 610 | */ |
Yajirushi | 0:3807ce385b2a | 611 | BOARDC_BNO055::BOARDC_BNO055(RawSerial *uart){ |
Yajirushi | 0:3807ce385b2a | 612 | ctrl = new BNO055_UART_CTRL(uart); |
Yajirushi | 0:3807ce385b2a | 613 | |
Yajirushi | 0:3807ce385b2a | 614 | scaleACC = 0.01f; // = 1 / 100 |
Yajirushi | 0:3807ce385b2a | 615 | scaleMAG = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 616 | scaleGYRO = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 617 | scaleEuler = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 618 | scaleTEMP = 1.0f; |
Yajirushi | 0:3807ce385b2a | 619 | scaleLIA = scaleACC; |
Yajirushi | 0:3807ce385b2a | 620 | scaleGV = scaleACC; |
Yajirushi | 0:3807ce385b2a | 621 | scaleQuaternion = 0.00006103515; // = 1 / (2^14) |
Yajirushi | 0:3807ce385b2a | 622 | |
Yajirushi | 0:3807ce385b2a | 623 | clkExt = false; |
Yajirushi | 0:3807ce385b2a | 624 | } |
Yajirushi | 0:3807ce385b2a | 625 | |
Yajirushi | 0:3807ce385b2a | 626 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 627 | * コンストラクタ (オーバーロード+3) |
Yajirushi | 0:3807ce385b2a | 628 | * I2Cで使用する際のコンストラクタ:ピン別名を指定する |
Yajirushi | 0:3807ce385b2a | 629 | */ |
Yajirushi | 0:3807ce385b2a | 630 | BOARDC_BNO055::BOARDC_BNO055(PinName sda, PinName scl, char addr, unsigned int freq){ |
Yajirushi | 0:3807ce385b2a | 631 | ctrl = new BNO055_I2C_CTRL(new I2C(sda, scl), addr, freq); |
Yajirushi | 0:3807ce385b2a | 632 | |
Yajirushi | 0:3807ce385b2a | 633 | scaleACC = 0.01f; // = 1 / 100 |
Yajirushi | 0:3807ce385b2a | 634 | scaleMAG = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 635 | scaleGYRO = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 636 | scaleEuler = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 637 | scaleTEMP = 1.0f; |
Yajirushi | 0:3807ce385b2a | 638 | scaleLIA = scaleACC; |
Yajirushi | 0:3807ce385b2a | 639 | scaleGV = scaleACC; |
Yajirushi | 0:3807ce385b2a | 640 | scaleQuaternion = 0.00006103515; // = 1 / (2^14) |
Yajirushi | 0:3807ce385b2a | 641 | |
Yajirushi | 0:3807ce385b2a | 642 | clkExt = false; |
Yajirushi | 0:3807ce385b2a | 643 | } |
Yajirushi | 0:3807ce385b2a | 644 | |
Yajirushi | 0:3807ce385b2a | 645 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 646 | * コンストラクタ (オーバーロード+3) |
Yajirushi | 0:3807ce385b2a | 647 | * I2Cで使用する際のコンストラクタ:I2Cクラスのインスタンスを指定する |
Yajirushi | 0:3807ce385b2a | 648 | */ |
Yajirushi | 0:3807ce385b2a | 649 | BOARDC_BNO055::BOARDC_BNO055(I2C* iic, char addr, unsigned int freq){ |
Yajirushi | 0:3807ce385b2a | 650 | ctrl = new BNO055_I2C_CTRL(iic, addr, freq); |
Yajirushi | 0:3807ce385b2a | 651 | |
Yajirushi | 0:3807ce385b2a | 652 | scaleACC = 0.01f; // = 1 / 100 |
Yajirushi | 0:3807ce385b2a | 653 | scaleMAG = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 654 | scaleGYRO = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 655 | scaleEuler = 0.0625f; // = 1 / 16 |
Yajirushi | 0:3807ce385b2a | 656 | scaleTEMP = 1.0f; |
Yajirushi | 0:3807ce385b2a | 657 | scaleLIA = scaleACC; |
Yajirushi | 0:3807ce385b2a | 658 | scaleGV = scaleACC; |
Yajirushi | 0:3807ce385b2a | 659 | scaleQuaternion = 0.00006103515; // = 1 / (2^14) |
Yajirushi | 0:3807ce385b2a | 660 | |
Yajirushi | 0:3807ce385b2a | 661 | clkExt = false; |
Yajirushi | 0:3807ce385b2a | 662 | } |
Yajirushi | 0:3807ce385b2a | 663 | |
Yajirushi | 0:3807ce385b2a | 664 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 665 | * デストラクタ |
Yajirushi | 0:3807ce385b2a | 666 | */ |
Yajirushi | 0:3807ce385b2a | 667 | BOARDC_BNO055::~BOARDC_BNO055(){ |
Yajirushi | 0:3807ce385b2a | 668 | delete ctrl; |
Yajirushi | 0:3807ce385b2a | 669 | } |
Yajirushi | 0:3807ce385b2a | 670 | |
Yajirushi | 0:3807ce385b2a | 671 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 672 | * デフォルト設定での初期化 |
Yajirushi | 0:3807ce385b2a | 673 | */ |
Yajirushi | 0:3807ce385b2a | 674 | char BOARDC_BNO055::initialize(bool resetIface){ |
Yajirushi | 0:3807ce385b2a | 675 | if(resetIface) ctrl->init(); |
Yajirushi | 0:3807ce385b2a | 676 | |
Yajirushi | 0:3807ce385b2a | 677 | //CONFIGモードに設定 |
Yajirushi | 0:3807ce385b2a | 678 | setOperation_CONFIG(); |
Yajirushi | 0:3807ce385b2a | 679 | |
Yajirushi | 0:3807ce385b2a | 680 | //外部発振子設定,セルフテストの実行,セルフテスト完了まで待つ |
Yajirushi | 0:3807ce385b2a | 681 | setSysTrigger(0x81); |
Yajirushi | 0:3807ce385b2a | 682 | |
Yajirushi | 0:3807ce385b2a | 683 | //加速度センサーレンジ設定(+-8G) |
Yajirushi | 0:3807ce385b2a | 684 | //地磁気や角速度センサーのレンジやバンドはFusionモードにより自動設定される |
Yajirushi | 0:3807ce385b2a | 685 | if(!setAccRange(16)) return -1; |
Yajirushi | 0:3807ce385b2a | 686 | |
Yajirushi | 0:3807ce385b2a | 687 | //各軸リマップ設定(BNO055の1pinマークが表側右下) |
Yajirushi | 0:3807ce385b2a | 688 | if(!setAxisRemap_topview_bottomright()) return -2; |
Yajirushi | 0:3807ce385b2a | 689 | |
Yajirushi | 0:3807ce385b2a | 690 | //単位系セット |
Yajirushi | 0:3807ce385b2a | 691 | //ORI_Android_Windows : Windows |
Yajirushi | 0:3807ce385b2a | 692 | //TEMP_Unit : Celcius |
Yajirushi | 0:3807ce385b2a | 693 | //EUL_Unit : Degree |
Yajirushi | 0:3807ce385b2a | 694 | //GYR_Unit : deg/s |
Yajirushi | 0:3807ce385b2a | 695 | //ACC_Unit : m/s^2 |
Yajirushi | 0:3807ce385b2a | 696 | if(!setUNIT_SEL(0x00)) return -3; |
Yajirushi | 0:3807ce385b2a | 697 | |
Yajirushi | 0:3807ce385b2a | 698 | //動作モード設定(9軸Fusionモード) |
Yajirushi | 0:3807ce385b2a | 699 | if(!setOperation_Fusion_NDOF()) return -4; |
Yajirushi | 0:3807ce385b2a | 700 | |
Yajirushi | 0:3807ce385b2a | 701 | return 0; |
Yajirushi | 0:3807ce385b2a | 702 | } |
Yajirushi | 0:3807ce385b2a | 703 | |
Yajirushi | 0:3807ce385b2a | 704 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 705 | * 通信で発生した最後のエラー番号を返す |
Yajirushi | 0:3807ce385b2a | 706 | */ |
Yajirushi | 0:3807ce385b2a | 707 | char BOARDC_BNO055::getIfaceLastError(){ |
Yajirushi | 0:3807ce385b2a | 708 | return ctrl->getLastError(); |
Yajirushi | 0:3807ce385b2a | 709 | } |
Yajirushi | 0:3807ce385b2a | 710 | |
Yajirushi | 0:3807ce385b2a | 711 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 712 | * 通信で発生した最後のエラーの通信バイト数を返す |
Yajirushi | 0:3807ce385b2a | 713 | */ |
Yajirushi | 0:3807ce385b2a | 714 | char BOARDC_BNO055::getIfaceLastLength(){ |
Yajirushi | 0:3807ce385b2a | 715 | return ctrl->getLastLength(); |
Yajirushi | 0:3807ce385b2a | 716 | } |
Yajirushi | 0:3807ce385b2a | 717 | |
Yajirushi | 0:3807ce385b2a | 718 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 719 | * ユーザー定義読み取り(1byte) |
Yajirushi | 0:3807ce385b2a | 720 | * レジスタを指定して値を直接読み取る |
Yajirushi | 0:3807ce385b2a | 721 | */ |
Yajirushi | 0:3807ce385b2a | 722 | char BOARDC_BNO055::customRead(bool isPage1, char regAddr){ |
Yajirushi | 0:3807ce385b2a | 723 | return ctrl->rr(isPage1, regAddr); |
Yajirushi | 0:3807ce385b2a | 724 | } |
Yajirushi | 0:3807ce385b2a | 725 | |
Yajirushi | 0:3807ce385b2a | 726 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 727 | * ユーザー定義読み取り(連続) |
Yajirushi | 0:3807ce385b2a | 728 | * レジスタを指定して値を直接読み取る |
Yajirushi | 0:3807ce385b2a | 729 | */ |
Yajirushi | 0:3807ce385b2a | 730 | char BOARDC_BNO055::customReadC(bool isPage1, char startRegAddr, char *receiveBytes, unsigned char length){ |
Yajirushi | 0:3807ce385b2a | 731 | return ctrl->rrc(isPage1, startRegAddr, receiveBytes, length); |
Yajirushi | 0:3807ce385b2a | 732 | } |
Yajirushi | 0:3807ce385b2a | 733 | |
Yajirushi | 0:3807ce385b2a | 734 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 735 | * ユーザー定義書き込み(1byte) |
Yajirushi | 0:3807ce385b2a | 736 | * レジスタを指定して値を直接書き込む |
Yajirushi | 0:3807ce385b2a | 737 | */ |
Yajirushi | 0:3807ce385b2a | 738 | char BOARDC_BNO055::customWrite(bool isPage1, char regAddr, char wBytes){ |
Yajirushi | 0:3807ce385b2a | 739 | return ctrl->wr(isPage1, regAddr, wBytes); |
Yajirushi | 0:3807ce385b2a | 740 | } |
Yajirushi | 0:3807ce385b2a | 741 | |
Yajirushi | 0:3807ce385b2a | 742 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 743 | * ユーザー定義書き込み(連続) |
Yajirushi | 0:3807ce385b2a | 744 | * レジスタを指定して値を直接書き込む |
Yajirushi | 0:3807ce385b2a | 745 | */ |
Yajirushi | 0:3807ce385b2a | 746 | char BOARDC_BNO055::customWriteC(bool isPage1, char startRegAddr, char *Bytes, unsigned char length){ |
Yajirushi | 0:3807ce385b2a | 747 | return ctrl->wrc(isPage1, startRegAddr, Bytes, length); |
Yajirushi | 0:3807ce385b2a | 748 | } |
Yajirushi | 0:3807ce385b2a | 749 | |
Yajirushi | 0:3807ce385b2a | 750 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 751 | * レジスタ:ページIDの現在値を読む |
Yajirushi | 0:3807ce385b2a | 752 | * 実際はコントロールクラスが保持している内部変数の値を返しているだけ |
Yajirushi | 0:3807ce385b2a | 753 | */ |
Yajirushi | 0:3807ce385b2a | 754 | char BOARDC_BNO055::getPage(){ |
Yajirushi | 0:3807ce385b2a | 755 | return ctrl->getNowPage(); |
Yajirushi | 0:3807ce385b2a | 756 | } |
Yajirushi | 0:3807ce385b2a | 757 | |
Yajirushi | 0:3807ce385b2a | 758 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 759 | * レジスタ:ページIDを変更する |
Yajirushi | 0:3807ce385b2a | 760 | * ページIDが現在値と同じであれば何もしない |
Yajirushi | 0:3807ce385b2a | 761 | */ |
Yajirushi | 0:3807ce385b2a | 762 | void BOARDC_BNO055::setPage(unsigned char pageNo){ |
Yajirushi | 0:3807ce385b2a | 763 | pageNo = (pageNo == 0) ? 0 : 1; |
Yajirushi | 0:3807ce385b2a | 764 | if(getPage() == pageNo) return; |
Yajirushi | 0:3807ce385b2a | 765 | ctrl->wr((getPage() == 0) ? true : false, BNO055_PAGE_ID, pageNo); |
Yajirushi | 0:3807ce385b2a | 766 | } |
Yajirushi | 0:3807ce385b2a | 767 | |
Yajirushi | 0:3807ce385b2a | 768 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 769 | * レジスタ:チップIDの値を取得する |
Yajirushi | 0:3807ce385b2a | 770 | */ |
Yajirushi | 0:3807ce385b2a | 771 | char BOARDC_BNO055::getChipID(){ |
Yajirushi | 0:3807ce385b2a | 772 | return ctrl->rr(0, BNO055P0_CHIP_ID); |
Yajirushi | 0:3807ce385b2a | 773 | } |
Yajirushi | 0:3807ce385b2a | 774 | |
Yajirushi | 0:3807ce385b2a | 775 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 776 | * レジスタ:加速度センサーIDの値を取得する |
Yajirushi | 0:3807ce385b2a | 777 | */ |
Yajirushi | 0:3807ce385b2a | 778 | char BOARDC_BNO055::getAccChipID(){ |
Yajirushi | 0:3807ce385b2a | 779 | return ctrl->rr(0, BNO055P0_ACC_ID); |
Yajirushi | 0:3807ce385b2a | 780 | } |
Yajirushi | 0:3807ce385b2a | 781 | |
Yajirushi | 0:3807ce385b2a | 782 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 783 | * レジスタ:地磁気センサーIDの値を取得する |
Yajirushi | 0:3807ce385b2a | 784 | */ |
Yajirushi | 0:3807ce385b2a | 785 | char BOARDC_BNO055::getMagChipID(){ |
Yajirushi | 0:3807ce385b2a | 786 | return ctrl->rr(0, BNO055P0_MAG_ID); |
Yajirushi | 0:3807ce385b2a | 787 | } |
Yajirushi | 0:3807ce385b2a | 788 | |
Yajirushi | 0:3807ce385b2a | 789 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 790 | * レジスタ:ジャイロセンサーIDの値を取得する |
Yajirushi | 0:3807ce385b2a | 791 | */ |
Yajirushi | 0:3807ce385b2a | 792 | char BOARDC_BNO055::getGyroChipID(){ |
Yajirushi | 0:3807ce385b2a | 793 | return ctrl->rr(0, BNO055P0_GYR_ID); |
Yajirushi | 0:3807ce385b2a | 794 | } |
Yajirushi | 0:3807ce385b2a | 795 | |
Yajirushi | 0:3807ce385b2a | 796 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 797 | * レジスタ:内部ソフトウェアリビジョンの値を取得する |
Yajirushi | 0:3807ce385b2a | 798 | */ |
Yajirushi | 0:3807ce385b2a | 799 | short BOARDC_BNO055::getRevision(){ |
Yajirushi | 0:3807ce385b2a | 800 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 801 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 802 | ctrl->rrc(0, BNO055P0_SW_REV_ID_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 803 | |
Yajirushi | 0:3807ce385b2a | 804 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 805 | } |
Yajirushi | 0:3807ce385b2a | 806 | |
Yajirushi | 0:3807ce385b2a | 807 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 808 | * レジスタ:内部ブートリビジョンの値を取得する |
Yajirushi | 0:3807ce385b2a | 809 | */ |
Yajirushi | 0:3807ce385b2a | 810 | char BOARDC_BNO055::getBootRevision(){ |
Yajirushi | 0:3807ce385b2a | 811 | return ctrl->rr(0, BNO055P0_BL_REV_ID); |
Yajirushi | 0:3807ce385b2a | 812 | } |
Yajirushi | 0:3807ce385b2a | 813 | |
Yajirushi | 0:3807ce385b2a | 814 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 815 | * 加速度センサーのRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 816 | */ |
Yajirushi | 0:3807ce385b2a | 817 | float BOARDC_BNO055::getAccScale(){ |
Yajirushi | 0:3807ce385b2a | 818 | return scaleACC; |
Yajirushi | 0:3807ce385b2a | 819 | } |
Yajirushi | 0:3807ce385b2a | 820 | |
Yajirushi | 0:3807ce385b2a | 821 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 822 | * 地磁気センサーのRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 823 | */ |
Yajirushi | 0:3807ce385b2a | 824 | float BOARDC_BNO055::getMagScale(){ |
Yajirushi | 0:3807ce385b2a | 825 | return scaleMAG; |
Yajirushi | 0:3807ce385b2a | 826 | } |
Yajirushi | 0:3807ce385b2a | 827 | |
Yajirushi | 0:3807ce385b2a | 828 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 829 | * 角速度センサーのRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 830 | */ |
Yajirushi | 0:3807ce385b2a | 831 | float BOARDC_BNO055::getGyroScale(){ |
Yajirushi | 0:3807ce385b2a | 832 | return scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 833 | } |
Yajirushi | 0:3807ce385b2a | 834 | |
Yajirushi | 0:3807ce385b2a | 835 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 836 | * 温度センサーのRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 837 | */ |
Yajirushi | 0:3807ce385b2a | 838 | float BOARDC_BNO055::getTempScale(){ |
Yajirushi | 0:3807ce385b2a | 839 | return scaleTEMP; |
Yajirushi | 0:3807ce385b2a | 840 | } |
Yajirushi | 0:3807ce385b2a | 841 | |
Yajirushi | 0:3807ce385b2a | 842 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 843 | * オイラー角のRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 844 | */ |
Yajirushi | 0:3807ce385b2a | 845 | float BOARDC_BNO055::getEulerScale(){ |
Yajirushi | 0:3807ce385b2a | 846 | return scaleEuler; |
Yajirushi | 0:3807ce385b2a | 847 | } |
Yajirushi | 0:3807ce385b2a | 848 | |
Yajirushi | 0:3807ce385b2a | 849 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 850 | * 加速度センサーの線形加速度のRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 851 | */ |
Yajirushi | 0:3807ce385b2a | 852 | float BOARDC_BNO055::getLinearScale(){ |
Yajirushi | 0:3807ce385b2a | 853 | return scaleLIA; |
Yajirushi | 0:3807ce385b2a | 854 | } |
Yajirushi | 0:3807ce385b2a | 855 | |
Yajirushi | 0:3807ce385b2a | 856 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 857 | * 重力ベクトルのRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 858 | */ |
Yajirushi | 0:3807ce385b2a | 859 | float BOARDC_BNO055::getGVScale(){ |
Yajirushi | 0:3807ce385b2a | 860 | return scaleGV; |
Yajirushi | 0:3807ce385b2a | 861 | } |
Yajirushi | 0:3807ce385b2a | 862 | |
Yajirushi | 0:3807ce385b2a | 863 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 864 | * 四元数のRAW値に乗算する係数を取得する |
Yajirushi | 0:3807ce385b2a | 865 | */ |
Yajirushi | 0:3807ce385b2a | 866 | double BOARDC_BNO055::getQuaternionScale(){ |
Yajirushi | 0:3807ce385b2a | 867 | return scaleQuaternion; |
Yajirushi | 0:3807ce385b2a | 868 | } |
Yajirushi | 0:3807ce385b2a | 869 | |
Yajirushi | 0:3807ce385b2a | 870 | |
Yajirushi | 0:3807ce385b2a | 871 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 872 | * 加速度センサの値を取得する |
Yajirushi | 0:3807ce385b2a | 873 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 874 | * &accX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 875 | * &accY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 876 | * &accZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 877 | */ |
Yajirushi | 0:3807ce385b2a | 878 | void BOARDC_BNO055::getAccDataAll(short &accX, short &accY, short &accZ){ |
Yajirushi | 0:3807ce385b2a | 879 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 880 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 881 | ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 882 | |
Yajirushi | 0:3807ce385b2a | 883 | accX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 884 | accY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 885 | accZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 886 | } |
Yajirushi | 0:3807ce385b2a | 887 | |
Yajirushi | 0:3807ce385b2a | 888 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 889 | * 加速度センサの値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 890 | */ |
Yajirushi | 0:3807ce385b2a | 891 | short BOARDC_BNO055::getAccDataX(){ |
Yajirushi | 0:3807ce385b2a | 892 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 893 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 894 | ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 895 | |
Yajirushi | 0:3807ce385b2a | 896 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 897 | } |
Yajirushi | 0:3807ce385b2a | 898 | |
Yajirushi | 0:3807ce385b2a | 899 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 900 | * 加速度センサの値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 901 | */ |
Yajirushi | 0:3807ce385b2a | 902 | short BOARDC_BNO055::getAccDataY(){ |
Yajirushi | 0:3807ce385b2a | 903 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 904 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 905 | ctrl->rrc(0, BNO055P0_ACC_DATA_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 906 | |
Yajirushi | 0:3807ce385b2a | 907 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 908 | } |
Yajirushi | 0:3807ce385b2a | 909 | |
Yajirushi | 0:3807ce385b2a | 910 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 911 | * 加速度センサの値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 912 | */ |
Yajirushi | 0:3807ce385b2a | 913 | short BOARDC_BNO055::getAccDataZ(){ |
Yajirushi | 0:3807ce385b2a | 914 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 915 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 916 | ctrl->rrc(0, BNO055P0_ACC_DATA_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 917 | |
Yajirushi | 0:3807ce385b2a | 918 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 919 | } |
Yajirushi | 0:3807ce385b2a | 920 | |
Yajirushi | 0:3807ce385b2a | 921 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 922 | * 地磁気センサの値を取得する |
Yajirushi | 0:3807ce385b2a | 923 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 924 | * &magX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 925 | * &magY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 926 | * &magZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 927 | */ |
Yajirushi | 0:3807ce385b2a | 928 | void BOARDC_BNO055::getMagDataAll(short &magX, short &magY, short &magZ){ |
Yajirushi | 0:3807ce385b2a | 929 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 930 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 931 | ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 932 | |
Yajirushi | 0:3807ce385b2a | 933 | magX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 934 | magY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 935 | magZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 936 | } |
Yajirushi | 0:3807ce385b2a | 937 | |
Yajirushi | 0:3807ce385b2a | 938 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 939 | * 地磁気センサの値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 940 | */ |
Yajirushi | 0:3807ce385b2a | 941 | short BOARDC_BNO055::getMagDataX(){ |
Yajirushi | 0:3807ce385b2a | 942 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 943 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 944 | ctrl->rrc(0, BNO055P0_MAG_DATA_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 945 | |
Yajirushi | 0:3807ce385b2a | 946 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 947 | } |
Yajirushi | 0:3807ce385b2a | 948 | |
Yajirushi | 0:3807ce385b2a | 949 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 950 | * 地磁気センサの値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 951 | */ |
Yajirushi | 0:3807ce385b2a | 952 | short BOARDC_BNO055::getMagDataY(){ |
Yajirushi | 0:3807ce385b2a | 953 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 954 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 955 | ctrl->rrc(0, BNO055P0_MAG_DATA_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 956 | |
Yajirushi | 0:3807ce385b2a | 957 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 958 | } |
Yajirushi | 0:3807ce385b2a | 959 | |
Yajirushi | 0:3807ce385b2a | 960 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 961 | * 地磁気センサの値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 962 | */ |
Yajirushi | 0:3807ce385b2a | 963 | short BOARDC_BNO055::getMagDataZ(){ |
Yajirushi | 0:3807ce385b2a | 964 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 965 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 966 | ctrl->rrc(0, BNO055P0_MAG_DATA_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 967 | |
Yajirushi | 0:3807ce385b2a | 968 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 969 | } |
Yajirushi | 0:3807ce385b2a | 970 | |
Yajirushi | 0:3807ce385b2a | 971 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 972 | * ジャイロセンサの値を取得する |
Yajirushi | 0:3807ce385b2a | 973 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 974 | * &gyroX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 975 | * &gyroY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 976 | * &gyroZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 977 | */ |
Yajirushi | 0:3807ce385b2a | 978 | void BOARDC_BNO055::getGyroDataAll(short &gyroX, short &gyroY, short &gyroZ){ |
Yajirushi | 0:3807ce385b2a | 979 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 980 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 981 | ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 982 | |
Yajirushi | 0:3807ce385b2a | 983 | gyroX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 984 | gyroY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 985 | gyroZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 986 | } |
Yajirushi | 0:3807ce385b2a | 987 | |
Yajirushi | 0:3807ce385b2a | 988 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 989 | * ジャイロセンサの値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 990 | */ |
Yajirushi | 0:3807ce385b2a | 991 | short BOARDC_BNO055::getGyroDataX(){ |
Yajirushi | 0:3807ce385b2a | 992 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 993 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 994 | ctrl->rrc(0, BNO055P0_GYR_DATA_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 995 | |
Yajirushi | 0:3807ce385b2a | 996 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 997 | } |
Yajirushi | 0:3807ce385b2a | 998 | |
Yajirushi | 0:3807ce385b2a | 999 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1000 | * ジャイロセンサの値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1001 | */ |
Yajirushi | 0:3807ce385b2a | 1002 | short BOARDC_BNO055::getGyroDataY(){ |
Yajirushi | 0:3807ce385b2a | 1003 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1004 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1005 | ctrl->rrc(0, BNO055P0_GYR_DATA_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1006 | |
Yajirushi | 0:3807ce385b2a | 1007 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1008 | } |
Yajirushi | 0:3807ce385b2a | 1009 | |
Yajirushi | 0:3807ce385b2a | 1010 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1011 | * ジャイロセンサの値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1012 | */ |
Yajirushi | 0:3807ce385b2a | 1013 | short BOARDC_BNO055::getGyroDataZ(){ |
Yajirushi | 0:3807ce385b2a | 1014 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1015 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1016 | ctrl->rrc(0, BNO055P0_GYR_DATA_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1017 | |
Yajirushi | 0:3807ce385b2a | 1018 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1019 | } |
Yajirushi | 0:3807ce385b2a | 1020 | |
Yajirushi | 0:3807ce385b2a | 1021 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1022 | * FusionSensing:オイラー角の値を取得する |
Yajirushi | 0:3807ce385b2a | 1023 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 1:7dbc931127c0 | 1024 | * Roll軸およびPitch軸が+-45degの範囲を超える場合、Yaw軸の値は不安定となる |
Yajirushi | 1:7dbc931127c0 | 1025 | * (その場合はgetEulerFromQを使用することで四元数からオイラー角を導出可能) |
Yajirushi | 0:3807ce385b2a | 1026 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1027 | * &E_heading: アドレス参照引数:関数実行後、この変数にYaw軸(heading)の値が格納される |
Yajirushi | 0:3807ce385b2a | 1028 | * &E_roll: アドレス参照引数:関数実行後、この変数にroll軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1029 | * &E_pitch: アドレス参照引数:関数実行後、この変数にpitch軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1030 | */ |
Yajirushi | 0:3807ce385b2a | 1031 | void BOARDC_BNO055::getEulerDataAll(short &E_heading, short &E_roll, short &E_pitch){ |
Yajirushi | 0:3807ce385b2a | 1032 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 1033 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1034 | ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 1035 | |
Yajirushi | 0:3807ce385b2a | 1036 | E_heading = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1037 | E_roll = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1038 | E_pitch = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1039 | } |
Yajirushi | 0:3807ce385b2a | 1040 | |
Yajirushi | 0:3807ce385b2a | 1041 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1042 | * FusionSensing:オイラー角の値(Yaw軸(heading)のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1043 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 0:3807ce385b2a | 1044 | */ |
Yajirushi | 0:3807ce385b2a | 1045 | short BOARDC_BNO055::getEulerDataHeading(){ |
Yajirushi | 0:3807ce385b2a | 1046 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1047 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1048 | ctrl->rrc(0, BNO055P0_EUL_HEADING_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1049 | |
Yajirushi | 0:3807ce385b2a | 1050 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1051 | } |
Yajirushi | 0:3807ce385b2a | 1052 | |
Yajirushi | 0:3807ce385b2a | 1053 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1054 | * getEulerDataHeading()のエイリアス |
Yajirushi | 0:3807ce385b2a | 1055 | * FusionSensing:オイラー角の値(Yaw軸(heading)のみ)を返す |
Yajirushi | 0:3807ce385b2a | 1056 | */ |
Yajirushi | 0:3807ce385b2a | 1057 | short BOARDC_BNO055::getEulerDataYaw(){ |
Yajirushi | 0:3807ce385b2a | 1058 | return getEulerDataHeading(); |
Yajirushi | 0:3807ce385b2a | 1059 | } |
Yajirushi | 0:3807ce385b2a | 1060 | |
Yajirushi | 0:3807ce385b2a | 1061 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1062 | * FusionSensing:オイラー角の値(pitch軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1063 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 0:3807ce385b2a | 1064 | */ |
Yajirushi | 0:3807ce385b2a | 1065 | short BOARDC_BNO055::getEulerDataRoll(){ |
Yajirushi | 0:3807ce385b2a | 1066 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1067 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1068 | ctrl->rrc(0, BNO055P0_EUL_ROLL_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1069 | |
Yajirushi | 0:3807ce385b2a | 1070 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1071 | } |
Yajirushi | 0:3807ce385b2a | 1072 | |
Yajirushi | 0:3807ce385b2a | 1073 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1074 | * FusionSensing:オイラー角の値(pitch軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1075 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 0:3807ce385b2a | 1076 | */ |
Yajirushi | 0:3807ce385b2a | 1077 | short BOARDC_BNO055::getEulerDataPitch(){ |
Yajirushi | 0:3807ce385b2a | 1078 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1079 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1080 | ctrl->rrc(0, BNO055P0_EUL_PITCH_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1081 | |
Yajirushi | 0:3807ce385b2a | 1082 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1083 | } |
Yajirushi | 0:3807ce385b2a | 1084 | |
Yajirushi | 0:3807ce385b2a | 1085 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1086 | * FusionSensing:9軸センサーのすべての値の値を取得する |
Yajirushi | 0:3807ce385b2a | 1087 | */ |
Yajirushi | 0:3807ce385b2a | 1088 | void BOARDC_BNO055::get9Axis(short *box){ |
Yajirushi | 0:3807ce385b2a | 1089 | char rsv[18]; |
Yajirushi | 0:3807ce385b2a | 1090 | ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 18); |
Yajirushi | 0:3807ce385b2a | 1091 | |
Yajirushi | 0:3807ce385b2a | 1092 | box[0] = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1093 | box[1] = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1094 | box[2] = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1095 | box[3] = (rsv[7] << 8) | rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1096 | box[4] = (rsv[9] << 8) | rsv[8]; |
Yajirushi | 0:3807ce385b2a | 1097 | box[5] = (rsv[11] << 8) | rsv[10]; |
Yajirushi | 0:3807ce385b2a | 1098 | box[6] = (rsv[13] << 8) | rsv[12]; |
Yajirushi | 0:3807ce385b2a | 1099 | box[7] = (rsv[15] << 8) | rsv[14]; |
Yajirushi | 0:3807ce385b2a | 1100 | box[8] = (rsv[17] << 8) | rsv[16]; |
Yajirushi | 0:3807ce385b2a | 1101 | } |
Yajirushi | 0:3807ce385b2a | 1102 | |
Yajirushi | 0:3807ce385b2a | 1103 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1104 | * FusionSensing:9軸センサーのすべての値とオイラー角のすべての値を取得する |
Yajirushi | 0:3807ce385b2a | 1105 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 0:3807ce385b2a | 1106 | */ |
Yajirushi | 0:3807ce385b2a | 1107 | void BOARDC_BNO055::get9AxisAndEUL(short *box){ |
Yajirushi | 0:3807ce385b2a | 1108 | char rsv[24]; |
Yajirushi | 0:3807ce385b2a | 1109 | ctrl->rrc(0, BNO055P0_ACC_DATA_X_LSB, rsv, 24); |
Yajirushi | 0:3807ce385b2a | 1110 | |
Yajirushi | 0:3807ce385b2a | 1111 | box[0] = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1112 | box[1] = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1113 | box[2] = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1114 | box[3] = (rsv[7] << 8) | rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1115 | box[4] = (rsv[9] << 8) | rsv[8]; |
Yajirushi | 0:3807ce385b2a | 1116 | box[5] = (rsv[11] << 8) | rsv[10]; |
Yajirushi | 0:3807ce385b2a | 1117 | box[6] = (rsv[13] << 8) | rsv[12]; |
Yajirushi | 0:3807ce385b2a | 1118 | box[7] = (rsv[15] << 8) | rsv[14]; |
Yajirushi | 0:3807ce385b2a | 1119 | box[8] = (rsv[17] << 8) | rsv[16]; |
Yajirushi | 0:3807ce385b2a | 1120 | box[9] = (rsv[19] << 8) | rsv[18]; |
Yajirushi | 0:3807ce385b2a | 1121 | box[10] = (rsv[21] << 8) | rsv[20]; |
Yajirushi | 0:3807ce385b2a | 1122 | box[11] = (rsv[23] << 8) | rsv[22]; |
Yajirushi | 0:3807ce385b2a | 1123 | } |
Yajirushi | 0:3807ce385b2a | 1124 | |
Yajirushi | 0:3807ce385b2a | 1125 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1126 | * FusionSensing:四元数(Quaternion)を取得する |
Yajirushi | 0:3807ce385b2a | 1127 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 0:3807ce385b2a | 1128 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1129 | * &q1, &q2, &q3, &q4:アドレス参照引数:関数実行後、この変数に四元数が格納される |
Yajirushi | 0:3807ce385b2a | 1130 | */ |
Yajirushi | 0:3807ce385b2a | 1131 | void BOARDC_BNO055::getQuaternion(short &q1, short &q2, short &q3, short &q4){ |
Yajirushi | 0:3807ce385b2a | 1132 | //連続8byte読み取り |
Yajirushi | 0:3807ce385b2a | 1133 | char rsv[8]; |
Yajirushi | 0:3807ce385b2a | 1134 | ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8); |
Yajirushi | 0:3807ce385b2a | 1135 | |
Yajirushi | 0:3807ce385b2a | 1136 | q1 = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1137 | q2 = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1138 | q3 = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1139 | q4 = (rsv[7] << 8) | rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1140 | } |
Yajirushi | 0:3807ce385b2a | 1141 | |
Yajirushi | 0:3807ce385b2a | 1142 | /* ================================================================== |
Yajirushi | 1:7dbc931127c0 | 1143 | * FusionSensing:四元数(Quaternion)を取得し、オイラー角を算出して返す |
Yajirushi | 1:7dbc931127c0 | 1144 | * FusionSensing機能(内部演算機能)を使用していない場合は不定の値を返す |
Yajirushi | 1:7dbc931127c0 | 1145 | * 出力数値範囲 -180.0 to 180.0[deg] CW(時計回り)- CCW(反時計回り)+ |
Yajirushi | 1:7dbc931127c0 | 1146 | * ------------------------------------------------------------------ |
Yajirushi | 1:7dbc931127c0 | 1147 | * &E_heading, &E_roll, &E_pitch:アドレス参照引数:関数実行後、この変数にオイラー角が格納される |
Yajirushi | 1:7dbc931127c0 | 1148 | */ |
Yajirushi | 1:7dbc931127c0 | 1149 | void BOARDC_BNO055::getEulerFromQ(double &E_heading, double &E_roll, double &E_pitch){ |
Yajirushi | 1:7dbc931127c0 | 1150 | //連続8byte読み取り |
Yajirushi | 1:7dbc931127c0 | 1151 | char rsv[8]; |
Yajirushi | 1:7dbc931127c0 | 1152 | ctrl->rrc(0, BNO055P0_QUA_DATA_W_LSB, rsv, 8); |
Yajirushi | 1:7dbc931127c0 | 1153 | |
Yajirushi | 1:7dbc931127c0 | 1154 | //四元数を実際の数値に変換(1 / 2^14を掛けるより2^14で割ったほうが早い・・・) |
Yajirushi | 1:7dbc931127c0 | 1155 | double q1, q2, q3, q4; |
Yajirushi | 1:7dbc931127c0 | 1156 | q1 = (double)((short)((rsv[1] << 8) | rsv[0]) / 16384.0); |
Yajirushi | 1:7dbc931127c0 | 1157 | q2 = (double)((short)((rsv[3] << 8) | rsv[2]) / 16384.0); |
Yajirushi | 1:7dbc931127c0 | 1158 | q3 = (double)((short)((rsv[5] << 8) | rsv[4]) / 16384.0); |
Yajirushi | 1:7dbc931127c0 | 1159 | q4 = (double)((short)((rsv[7] << 8) | rsv[6]) / 16384.0); |
Yajirushi | 1:7dbc931127c0 | 1160 | |
Yajirushi | 1:7dbc931127c0 | 1161 | //四元数からオイラー角に変換 |
Yajirushi | 1:7dbc931127c0 | 1162 | //ref: Wikipedia |
Yajirushi | 1:7dbc931127c0 | 1163 | //Conversion between quaternions and Euler angles(Quaternion to Euler Angles Conversion) |
Yajirushi | 1:7dbc931127c0 | 1164 | double q3q3 = q3 * q3; |
Yajirushi | 1:7dbc931127c0 | 1165 | |
Yajirushi | 1:7dbc931127c0 | 1166 | double m1 = +2.0 * (q1 * q2 + q3 * q4); |
Yajirushi | 1:7dbc931127c0 | 1167 | double m2 = +1.0 - 2.0 * (q2 * q2 + q3q3); |
Yajirushi | 1:7dbc931127c0 | 1168 | E_roll = atan2(m1, m2) * 57.2957795131; |
Yajirushi | 1:7dbc931127c0 | 1169 | |
Yajirushi | 1:7dbc931127c0 | 1170 | m1 = +2.0 * (q1 * q3 - q4 * q2); |
Yajirushi | 1:7dbc931127c0 | 1171 | m1 = (m1 > 1.0)? 1.0 : m1; |
Yajirushi | 1:7dbc931127c0 | 1172 | m1 = (m1 < -1.0)? -1.0 : m1; |
Yajirushi | 1:7dbc931127c0 | 1173 | E_pitch = asin(m1) * 57.2957795131; |
Yajirushi | 1:7dbc931127c0 | 1174 | |
Yajirushi | 1:7dbc931127c0 | 1175 | m1 = +2.0 * (q1 * q4 + q2 * q3); |
Yajirushi | 1:7dbc931127c0 | 1176 | m2 = +1.0 - 2.0 * (q3q3 + q4 * q4); |
Yajirushi | 1:7dbc931127c0 | 1177 | E_heading = atan2(m1, m2) * 57.2957795131; |
Yajirushi | 1:7dbc931127c0 | 1178 | } |
Yajirushi | 1:7dbc931127c0 | 1179 | |
Yajirushi | 1:7dbc931127c0 | 1180 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1181 | * 線形加速度(LinearAcceleration)での加速度センサの値を取得する |
Yajirushi | 0:3807ce385b2a | 1182 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1183 | * &L_accX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1184 | * &L_accY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1185 | * &L_accZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1186 | */ |
Yajirushi | 0:3807ce385b2a | 1187 | void BOARDC_BNO055::getLinearAccDataAll(short &L_accX, short &L_accY, short &L_accZ){ |
Yajirushi | 0:3807ce385b2a | 1188 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 1189 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1190 | ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 1191 | |
Yajirushi | 0:3807ce385b2a | 1192 | L_accX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1193 | L_accY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1194 | L_accZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1195 | } |
Yajirushi | 0:3807ce385b2a | 1196 | |
Yajirushi | 0:3807ce385b2a | 1197 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1198 | * 線形加速度(LinearAcceleration)での加速度センサの値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1199 | */ |
Yajirushi | 0:3807ce385b2a | 1200 | short BOARDC_BNO055::getLinearAccDataX(){ |
Yajirushi | 0:3807ce385b2a | 1201 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1202 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1203 | ctrl->rrc(0, BNO055P0_LIA_DATA_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1204 | |
Yajirushi | 0:3807ce385b2a | 1205 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1206 | } |
Yajirushi | 0:3807ce385b2a | 1207 | |
Yajirushi | 0:3807ce385b2a | 1208 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1209 | * 線形加速度(LinearAcceleration)での加速度センサの値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1210 | */ |
Yajirushi | 0:3807ce385b2a | 1211 | short BOARDC_BNO055::getLinearAccDataY(){ |
Yajirushi | 0:3807ce385b2a | 1212 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1213 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1214 | ctrl->rrc(0, BNO055P0_LIA_DATA_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1215 | |
Yajirushi | 0:3807ce385b2a | 1216 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1217 | } |
Yajirushi | 0:3807ce385b2a | 1218 | |
Yajirushi | 0:3807ce385b2a | 1219 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1220 | * 線形加速度(LinearAcceleration)での加速度センサの値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1221 | */ |
Yajirushi | 0:3807ce385b2a | 1222 | short BOARDC_BNO055::getLinearAccDataZ(){ |
Yajirushi | 0:3807ce385b2a | 1223 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1224 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1225 | ctrl->rrc(0, BNO055P0_LIA_DATA_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1226 | |
Yajirushi | 0:3807ce385b2a | 1227 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1228 | } |
Yajirushi | 0:3807ce385b2a | 1229 | |
Yajirushi | 0:3807ce385b2a | 1230 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1231 | * 重力ベクトル情報を取得する |
Yajirushi | 0:3807ce385b2a | 1232 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1233 | * &gvX: アドレス参照引数:関数実行後、この変数にX軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1234 | * &gvY: アドレス参照引数:関数実行後、この変数にY軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1235 | * &gvZ: アドレス参照引数:関数実行後、この変数にZ軸の値が格納される |
Yajirushi | 0:3807ce385b2a | 1236 | */ |
Yajirushi | 0:3807ce385b2a | 1237 | void BOARDC_BNO055::getGVectorDataAll(short &gvX, short &gvY, short &gvZ){ |
Yajirushi | 0:3807ce385b2a | 1238 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 1239 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 1240 | ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 1241 | |
Yajirushi | 0:3807ce385b2a | 1242 | gvX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1243 | gvY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1244 | gvZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 1245 | } |
Yajirushi | 0:3807ce385b2a | 1246 | |
Yajirushi | 0:3807ce385b2a | 1247 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1248 | * 重力ベクトル情報(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1249 | */ |
Yajirushi | 0:3807ce385b2a | 1250 | short BOARDC_BNO055::getGVectorDataX(){ |
Yajirushi | 0:3807ce385b2a | 1251 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1252 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1253 | ctrl->rrc(0, BNO055P0_GRV_DATA_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1254 | |
Yajirushi | 0:3807ce385b2a | 1255 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1256 | } |
Yajirushi | 0:3807ce385b2a | 1257 | |
Yajirushi | 0:3807ce385b2a | 1258 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1259 | * 重力ベクトル情報(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1260 | */ |
Yajirushi | 0:3807ce385b2a | 1261 | short BOARDC_BNO055::getGVectorDataY(){ |
Yajirushi | 0:3807ce385b2a | 1262 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1263 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1264 | ctrl->rrc(0, BNO055P0_GRV_DATA_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1265 | |
Yajirushi | 0:3807ce385b2a | 1266 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1267 | } |
Yajirushi | 0:3807ce385b2a | 1268 | |
Yajirushi | 0:3807ce385b2a | 1269 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1270 | * 重力ベクトル情報(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1271 | */ |
Yajirushi | 0:3807ce385b2a | 1272 | short BOARDC_BNO055::getGVectorDataZ(){ |
Yajirushi | 0:3807ce385b2a | 1273 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 1274 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 1275 | ctrl->rrc(0, BNO055P0_GRV_DATA_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 1276 | |
Yajirushi | 0:3807ce385b2a | 1277 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 1278 | } |
Yajirushi | 0:3807ce385b2a | 1279 | |
Yajirushi | 0:3807ce385b2a | 1280 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1281 | * センサー内部温度情報を取得する |
Yajirushi | 0:3807ce385b2a | 1282 | * 内部温度のセンサーは2つあり、どちらか一方のみの温度を返す |
Yajirushi | 0:3807ce385b2a | 1283 | * (設定レジスタTEMP_SOURCEにて選択可) |
Yajirushi | 0:3807ce385b2a | 1284 | * (setTempSource(bool)にて選択可) |
Yajirushi | 0:3807ce385b2a | 1285 | */ |
Yajirushi | 0:3807ce385b2a | 1286 | char BOARDC_BNO055::getTemperature(){ |
Yajirushi | 0:3807ce385b2a | 1287 | return ctrl->rr(0, BNO055P0_TEMP); |
Yajirushi | 0:3807ce385b2a | 1288 | } |
Yajirushi | 0:3807ce385b2a | 1289 | |
Yajirushi | 0:3807ce385b2a | 1290 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1291 | * キャリブレーション(補正)の状態を取得する |
Yajirushi | 0:3807ce385b2a | 1292 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1293 | * &sys: アドレス参照引数:関数実行後、この変数にsystem補正の状態が格納される |
Yajirushi | 0:3807ce385b2a | 1294 | * &acc: アドレス参照引数:関数実行後、この変数に加速度センサー補正の状態が格納される |
Yajirushi | 0:3807ce385b2a | 1295 | * &mag: アドレス参照引数:関数実行後、この変数に地磁気センサー補正の状態が格納される |
Yajirushi | 0:3807ce385b2a | 1296 | * &gyro: アドレス参照引数:関数実行後、この変数に角速度センサー補正の状態が格納される |
Yajirushi | 0:3807ce385b2a | 1297 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1298 | * 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため |
Yajirushi | 0:3807ce385b2a | 1299 | * 値は0[%],32[%],66[%],100[%]のいずれか |
Yajirushi | 0:3807ce385b2a | 1300 | */ |
Yajirushi | 0:3807ce385b2a | 1301 | void BOARDC_BNO055::getCalibStatusAll(char &sys, char &acc, char &mag, char &gyro){ |
Yajirushi | 0:3807ce385b2a | 1302 | char rv = ctrl->rr(0, BNO055P0_CALIB_STAT); |
Yajirushi | 0:3807ce385b2a | 1303 | sys = (((rv & 0xC0) >> 6) * 34); |
Yajirushi | 0:3807ce385b2a | 1304 | gyro = (((rv & 0x30) >> 4) * 34); |
Yajirushi | 0:3807ce385b2a | 1305 | acc = (((rv & 0x0C) >> 2) * 34); |
Yajirushi | 0:3807ce385b2a | 1306 | mag = ((rv & 0x03) * 34); |
Yajirushi | 0:3807ce385b2a | 1307 | sys -= (sys == 0) ? 0 : 2; |
Yajirushi | 0:3807ce385b2a | 1308 | gyro -= (gyro == 0) ? 0 : 2; |
Yajirushi | 0:3807ce385b2a | 1309 | acc -= (acc == 0) ? 0 : 2; |
Yajirushi | 0:3807ce385b2a | 1310 | mag -= (mag == 0) ? 0 : 2; |
Yajirushi | 0:3807ce385b2a | 1311 | } |
Yajirushi | 0:3807ce385b2a | 1312 | |
Yajirushi | 0:3807ce385b2a | 1313 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1314 | * キャリブレーション(補正)の状態(systemのみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1315 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1316 | * 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため |
Yajirushi | 0:3807ce385b2a | 1317 | * 値は0[%],32[%],66[%],100[%]のいずれか |
Yajirushi | 0:3807ce385b2a | 1318 | */ |
Yajirushi | 0:3807ce385b2a | 1319 | char BOARDC_BNO055::getCalibStatusSys(){ |
Yajirushi | 0:3807ce385b2a | 1320 | char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0xC0) >> 6) * 34) - 2; |
Yajirushi | 0:3807ce385b2a | 1321 | return (ret < 0) ? 0 : ret; |
Yajirushi | 0:3807ce385b2a | 1322 | } |
Yajirushi | 0:3807ce385b2a | 1323 | |
Yajirushi | 0:3807ce385b2a | 1324 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1325 | * キャリブレーション(補正)の状態(加速度センサーのみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1326 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1327 | * 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため |
Yajirushi | 0:3807ce385b2a | 1328 | * 値は0[%],32[%],66[%],100[%]のいずれか |
Yajirushi | 0:3807ce385b2a | 1329 | */ |
Yajirushi | 0:3807ce385b2a | 1330 | char BOARDC_BNO055::getCalibStatusAcc(){ |
Yajirushi | 0:3807ce385b2a | 1331 | char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x0C) >> 2) * 34) - 2; |
Yajirushi | 0:3807ce385b2a | 1332 | return (ret < 0) ? 0 : ret; |
Yajirushi | 0:3807ce385b2a | 1333 | } |
Yajirushi | 0:3807ce385b2a | 1334 | |
Yajirushi | 0:3807ce385b2a | 1335 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1336 | * キャリブレーション(補正)の状態(地磁気センサーのみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1337 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1338 | * 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため |
Yajirushi | 0:3807ce385b2a | 1339 | * 値は0[%],32[%],66[%],100[%]のいずれか |
Yajirushi | 0:3807ce385b2a | 1340 | */ |
Yajirushi | 0:3807ce385b2a | 1341 | char BOARDC_BNO055::getCalibStatusMag(){ |
Yajirushi | 0:3807ce385b2a | 1342 | char ret = ((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x03) * 34) - 2; |
Yajirushi | 0:3807ce385b2a | 1343 | return (ret < 0) ? 0 : ret; |
Yajirushi | 0:3807ce385b2a | 1344 | } |
Yajirushi | 0:3807ce385b2a | 1345 | |
Yajirushi | 0:3807ce385b2a | 1346 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1347 | * キャリブレーション(補正)の状態(角速度センサーのみ)を取得する |
Yajirushi | 0:3807ce385b2a | 1348 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1349 | * 各補正情報は0 - 100[%]の百分率で返答される。それぞれ2bitであるため |
Yajirushi | 0:3807ce385b2a | 1350 | * 値は0[%],32[%],66[%],100[%]のいずれか |
Yajirushi | 0:3807ce385b2a | 1351 | */ |
Yajirushi | 0:3807ce385b2a | 1352 | char BOARDC_BNO055::getCalibStatusGyro(){ |
Yajirushi | 0:3807ce385b2a | 1353 | char ret = (((ctrl->rr(0, BNO055P0_CALIB_STAT) & 0x30) >> 4) * 34) - 2; |
Yajirushi | 0:3807ce385b2a | 1354 | return (ret < 0) ? 0 : ret; |
Yajirushi | 0:3807ce385b2a | 1355 | } |
Yajirushi | 0:3807ce385b2a | 1356 | |
Yajirushi | 0:3807ce385b2a | 1357 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1358 | * システムおよびセンサーのセルフテストの実行結果を取得する |
Yajirushi | 0:3807ce385b2a | 1359 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1360 | * returns: |
Yajirushi | 0:3807ce385b2a | 1361 | * 1bit目:加速度センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常)) |
Yajirushi | 0:3807ce385b2a | 1362 | * 2bit目:地磁気センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常)) |
Yajirushi | 0:3807ce385b2a | 1363 | * 3bit目:角速度センサーのセルフテストの実行結果(0:failed(異常), 1:passed(正常)) |
Yajirushi | 0:3807ce385b2a | 1364 | * 4bit目:内部マイコンのセルフテストの実行結果(0:failed(異常), 1:passed(正常)) |
Yajirushi | 0:3807ce385b2a | 1365 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1366 | * 例:戻り値 0x0D (0b00001101) |
Yajirushi | 0:3807ce385b2a | 1367 | * >>加速度センサー:正常(1) |
Yajirushi | 0:3807ce385b2a | 1368 | * >>地磁気センサー:異常(0) |
Yajirushi | 0:3807ce385b2a | 1369 | * >>角速度センサー:正常(1) |
Yajirushi | 0:3807ce385b2a | 1370 | * >>内部マイコン:正常(1) |
Yajirushi | 0:3807ce385b2a | 1371 | */ |
Yajirushi | 0:3807ce385b2a | 1372 | char BOARDC_BNO055::getSelfTestResultAll(){ |
Yajirushi | 0:3807ce385b2a | 1373 | return ctrl->rr(0, BNO055P0_ST_RESULT); |
Yajirushi | 0:3807ce385b2a | 1374 | } |
Yajirushi | 0:3807ce385b2a | 1375 | |
Yajirushi | 0:3807ce385b2a | 1376 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1377 | * 内部マイコン(BNO055のMCU)のセルフテストの実行結果を取得する |
Yajirushi | 0:3807ce385b2a | 1378 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1379 | * returns: |
Yajirushi | 0:3807ce385b2a | 1380 | * false:failed(異常) |
Yajirushi | 0:3807ce385b2a | 1381 | * true:passed(正常) |
Yajirushi | 0:3807ce385b2a | 1382 | */ |
Yajirushi | 0:3807ce385b2a | 1383 | bool BOARDC_BNO055::getSelfTestResultMCU(){ |
Yajirushi | 0:3807ce385b2a | 1384 | return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x08) >> 3) == 1); |
Yajirushi | 0:3807ce385b2a | 1385 | } |
Yajirushi | 0:3807ce385b2a | 1386 | |
Yajirushi | 0:3807ce385b2a | 1387 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1388 | * 加速度センサーのセルフテストの実行結果を取得する |
Yajirushi | 0:3807ce385b2a | 1389 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1390 | * returns: |
Yajirushi | 0:3807ce385b2a | 1391 | * false:failed(異常) |
Yajirushi | 0:3807ce385b2a | 1392 | * true:passed(正常) |
Yajirushi | 0:3807ce385b2a | 1393 | */ |
Yajirushi | 0:3807ce385b2a | 1394 | bool BOARDC_BNO055::getSelfTestResultAcc(){ |
Yajirushi | 0:3807ce385b2a | 1395 | return ((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x01) == 1); |
Yajirushi | 0:3807ce385b2a | 1396 | } |
Yajirushi | 0:3807ce385b2a | 1397 | |
Yajirushi | 0:3807ce385b2a | 1398 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1399 | * 地磁気センサーのセルフテストの実行結果を取得する |
Yajirushi | 0:3807ce385b2a | 1400 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1401 | * returns: |
Yajirushi | 0:3807ce385b2a | 1402 | * false:failed(異常) |
Yajirushi | 0:3807ce385b2a | 1403 | * true:passed(正常) |
Yajirushi | 0:3807ce385b2a | 1404 | */ |
Yajirushi | 0:3807ce385b2a | 1405 | bool BOARDC_BNO055::getSelfTestResultMag(){ |
Yajirushi | 0:3807ce385b2a | 1406 | return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x02) >> 1) == 1); |
Yajirushi | 0:3807ce385b2a | 1407 | } |
Yajirushi | 0:3807ce385b2a | 1408 | |
Yajirushi | 0:3807ce385b2a | 1409 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1410 | * 地磁気センサーのセルフテストの実行結果を取得する |
Yajirushi | 0:3807ce385b2a | 1411 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1412 | * returns: |
Yajirushi | 0:3807ce385b2a | 1413 | * false:failed(異常) |
Yajirushi | 0:3807ce385b2a | 1414 | * true:passed(正常) |
Yajirushi | 0:3807ce385b2a | 1415 | */ |
Yajirushi | 0:3807ce385b2a | 1416 | bool BOARDC_BNO055::getSelfTestResultGyro(){ |
Yajirushi | 0:3807ce385b2a | 1417 | return (((ctrl->rr(0, BNO055P0_ST_RESULT) & 0x04) >> 2) == 1); |
Yajirushi | 0:3807ce385b2a | 1418 | } |
Yajirushi | 0:3807ce385b2a | 1419 | |
Yajirushi | 0:3807ce385b2a | 1420 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1421 | * 発生している割り込みステータス情報を取得する |
Yajirushi | 0:3807ce385b2a | 1422 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1423 | * returns: |
Yajirushi | 0:3807ce385b2a | 1424 | * 3bit目:GYRO_AM(角速度AnyMotion発生)(1:発生中, 0:なし) |
Yajirushi | 0:3807ce385b2a | 1425 | * 4bit目:GYR_HIGH_RATE(角速度ハイレート発生)(1:発生中, 0:なし) |
Yajirushi | 0:3807ce385b2a | 1426 | * 6bit目:ACC_HIGH_G(加速度急加速発生)(1:発生中, 0:なし) |
Yajirushi | 0:3807ce385b2a | 1427 | * 7bit目:ACC_AM(角速度AnyMotion発生)(1:発生中, 0:なし) |
Yajirushi | 0:3807ce385b2a | 1428 | * 8bit目:ACC_NM(角速度NoMotion発生)(1:発生中, 0:なし) |
Yajirushi | 0:3807ce385b2a | 1429 | */ |
Yajirushi | 0:3807ce385b2a | 1430 | char BOARDC_BNO055::triggeredIntALL(){ |
Yajirushi | 0:3807ce385b2a | 1431 | return ctrl->rr(0, BNO055P0_INT_STA); |
Yajirushi | 0:3807ce385b2a | 1432 | } |
Yajirushi | 0:3807ce385b2a | 1433 | |
Yajirushi | 0:3807ce385b2a | 1434 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1435 | * 発生している割り込みステータス情報(ACC_NM)を取得する |
Yajirushi | 0:3807ce385b2a | 1436 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1437 | * returns: |
Yajirushi | 0:3807ce385b2a | 1438 | * true:ACC_NMトリガー発生中 |
Yajirushi | 0:3807ce385b2a | 1439 | * false:ACC_NMトリガーなし |
Yajirushi | 0:3807ce385b2a | 1440 | */ |
Yajirushi | 0:3807ce385b2a | 1441 | bool BOARDC_BNO055::triggeredACC_NM(){ |
Yajirushi | 0:3807ce385b2a | 1442 | return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x80) >> 7) == 1); |
Yajirushi | 0:3807ce385b2a | 1443 | } |
Yajirushi | 0:3807ce385b2a | 1444 | |
Yajirushi | 0:3807ce385b2a | 1445 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1446 | * 発生している割り込みステータス情報(ACC_AM)を取得する |
Yajirushi | 0:3807ce385b2a | 1447 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1448 | * returns: |
Yajirushi | 0:3807ce385b2a | 1449 | * true:ACC_AMトリガー発生中 |
Yajirushi | 0:3807ce385b2a | 1450 | * false:ACC_AMトリガーなし |
Yajirushi | 0:3807ce385b2a | 1451 | */ |
Yajirushi | 0:3807ce385b2a | 1452 | bool BOARDC_BNO055::triggeredACC_AM(){ |
Yajirushi | 0:3807ce385b2a | 1453 | return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x40) >> 6) == 1); |
Yajirushi | 0:3807ce385b2a | 1454 | } |
Yajirushi | 0:3807ce385b2a | 1455 | |
Yajirushi | 0:3807ce385b2a | 1456 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1457 | * 発生している割り込みステータス情報(ACC_HIGH_G)を取得する |
Yajirushi | 0:3807ce385b2a | 1458 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1459 | * returns: |
Yajirushi | 0:3807ce385b2a | 1460 | * true:ACC_HIGH_Gトリガー発生中 |
Yajirushi | 0:3807ce385b2a | 1461 | * false:ACC_HIGH_Gトリガーなし |
Yajirushi | 0:3807ce385b2a | 1462 | */ |
Yajirushi | 0:3807ce385b2a | 1463 | bool BOARDC_BNO055::triggeredACC_HIGH_G(){ |
Yajirushi | 0:3807ce385b2a | 1464 | return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x20) >> 5) == 1); |
Yajirushi | 0:3807ce385b2a | 1465 | } |
Yajirushi | 0:3807ce385b2a | 1466 | |
Yajirushi | 0:3807ce385b2a | 1467 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1468 | * 発生している割り込みステータス情報(GYR_HIGH_RATE)を取得する |
Yajirushi | 0:3807ce385b2a | 1469 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1470 | * returns: |
Yajirushi | 0:3807ce385b2a | 1471 | * true:GYR_HIGH_RATEトリガー発生中 |
Yajirushi | 0:3807ce385b2a | 1472 | * false:GYR_HIGH_RATEトリガーなし |
Yajirushi | 0:3807ce385b2a | 1473 | */ |
Yajirushi | 0:3807ce385b2a | 1474 | bool BOARDC_BNO055::triggeredGYR_HIGH_RATE(){ |
Yajirushi | 0:3807ce385b2a | 1475 | return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x08) >> 3) == 1); |
Yajirushi | 0:3807ce385b2a | 1476 | } |
Yajirushi | 0:3807ce385b2a | 1477 | |
Yajirushi | 0:3807ce385b2a | 1478 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1479 | * 発生している割り込みステータス情報(GYRO_AM)を取得する |
Yajirushi | 0:3807ce385b2a | 1480 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1481 | * returns: |
Yajirushi | 0:3807ce385b2a | 1482 | * true:GYRO_AMトリガー発生中 |
Yajirushi | 0:3807ce385b2a | 1483 | * false:GYRO_AMトリガーなし |
Yajirushi | 0:3807ce385b2a | 1484 | */ |
Yajirushi | 0:3807ce385b2a | 1485 | bool BOARDC_BNO055::triggeredGYRO_AM(){ |
Yajirushi | 0:3807ce385b2a | 1486 | return (((ctrl->rr(0, BNO055P0_INT_STA) & 0x04) >> 2) == 1); |
Yajirushi | 0:3807ce385b2a | 1487 | } |
Yajirushi | 0:3807ce385b2a | 1488 | |
Yajirushi | 0:3807ce385b2a | 1489 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1490 | * BNO055のシステムクロック固定情報を取得する |
Yajirushi | 0:3807ce385b2a | 1491 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1492 | * returns: |
Yajirushi | 0:3807ce385b2a | 1493 | * true:設定によって固定されている(SYS_TRIGGEレジスタのCLK_SEL) |
Yajirushi | 0:3807ce385b2a | 1494 | * false:設定されていない(内部か外部選択可) |
Yajirushi | 0:3807ce385b2a | 1495 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1496 | * setSys_ExternalCrystal(bool)で設定可 |
Yajirushi | 0:3807ce385b2a | 1497 | */ |
Yajirushi | 0:3807ce385b2a | 1498 | bool BOARDC_BNO055::isSystemClockFixed(){ |
Yajirushi | 0:3807ce385b2a | 1499 | return (ctrl->rr(0, BNO055P0_SYS_CLK_STATUS) == 1); |
Yajirushi | 0:3807ce385b2a | 1500 | } |
Yajirushi | 0:3807ce385b2a | 1501 | |
Yajirushi | 0:3807ce385b2a | 1502 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1503 | * BNO055のシステムステータスを取得する |
Yajirushi | 0:3807ce385b2a | 1504 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1505 | * returns: |
Yajirushi | 0:3807ce385b2a | 1506 | * 0: システム待機状態 |
Yajirushi | 0:3807ce385b2a | 1507 | * 1: システムエラー |
Yajirushi | 0:3807ce385b2a | 1508 | * 2: ペリフェラル初期化中 |
Yajirushi | 0:3807ce385b2a | 1509 | * 3: システム初期化中 |
Yajirushi | 0:3807ce385b2a | 1510 | * 4: セルフテスト実行中 |
Yajirushi | 0:3807ce385b2a | 1511 | * 5: 起動中(Fusionアルゴリズム起動中) |
Yajirushi | 0:3807ce385b2a | 1512 | * 6: 起動中(Fusionアルゴリズムなし) |
Yajirushi | 0:3807ce385b2a | 1513 | */ |
Yajirushi | 0:3807ce385b2a | 1514 | char BOARDC_BNO055::getSystemStatus(){ |
Yajirushi | 0:3807ce385b2a | 1515 | return ctrl->rr(0, BNO055P0_SYS_STATUS); |
Yajirushi | 0:3807ce385b2a | 1516 | } |
Yajirushi | 0:3807ce385b2a | 1517 | |
Yajirushi | 0:3807ce385b2a | 1518 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1519 | * BNO055のシステムエラー情報を取得する |
Yajirushi | 0:3807ce385b2a | 1520 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1521 | * returns: |
Yajirushi | 0:3807ce385b2a | 1522 | * 0: エラーなし |
Yajirushi | 0:3807ce385b2a | 1523 | * 1: ペリフェラル初期化エラー |
Yajirushi | 0:3807ce385b2a | 1524 | * 2: システム初期化エラー |
Yajirushi | 0:3807ce385b2a | 1525 | * 3: セルフテスト結果不調 |
Yajirushi | 0:3807ce385b2a | 1526 | * 4: レジスタマップエラー(値の範囲外) |
Yajirushi | 0:3807ce385b2a | 1527 | * 5: レジスタマップエラー(アドレスの範囲外) |
Yajirushi | 0:3807ce385b2a | 1528 | * 6: レジスタマップエラー(書き込み不能) |
Yajirushi | 0:3807ce385b2a | 1529 | * 7: BNO005LowPowerモードにつき指定モード実行不可 |
Yajirushi | 0:3807ce385b2a | 1530 | * 8: 加速度センサーPowerMode実行不可 |
Yajirushi | 0:3807ce385b2a | 1531 | * 9: Fusionアルゴリズム設定エラー |
Yajirushi | 0:3807ce385b2a | 1532 | * 10: 各センサー設定エラー |
Yajirushi | 0:3807ce385b2a | 1533 | */ |
Yajirushi | 0:3807ce385b2a | 1534 | char BOARDC_BNO055::getSystemError(){ |
Yajirushi | 0:3807ce385b2a | 1535 | return ctrl->rr(0, BNO055P0_SYS_ERR); |
Yajirushi | 0:3807ce385b2a | 1536 | } |
Yajirushi | 0:3807ce385b2a | 1537 | |
Yajirushi | 0:3807ce385b2a | 1538 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1539 | * システムの設定単位系を取得する |
Yajirushi | 0:3807ce385b2a | 1540 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1541 | * returns: |
Yajirushi | 0:3807ce385b2a | 1542 | * 1bit目: 加速度センサー単位系(0:m/s^2, 1:mg) |
Yajirushi | 0:3807ce385b2a | 1543 | * 2bit目: 角速度センサー単位系(0:deg/s, 1:rad/s) |
Yajirushi | 0:3807ce385b2a | 1544 | * 3bit目: オイラー角単位系(0:deg, 1:rad) |
Yajirushi | 0:3807ce385b2a | 1545 | * 5bit目: 温度単位系(0:摂氏, 1:華氏) |
Yajirushi | 0:3807ce385b2a | 1546 | * 7bit目: 出力設定(0:Windows, 1:Android) |
Yajirushi | 0:3807ce385b2a | 1547 | */ |
Yajirushi | 0:3807ce385b2a | 1548 | char BOARDC_BNO055::getUNIT_SEL(){ |
Yajirushi | 0:3807ce385b2a | 1549 | return ctrl->rr(0, BNO055P0_UNIT_SEL); |
Yajirushi | 0:3807ce385b2a | 1550 | } |
Yajirushi | 0:3807ce385b2a | 1551 | |
Yajirushi | 0:3807ce385b2a | 1552 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1553 | * システムの設定単位系を設定する |
Yajirushi | 0:3807ce385b2a | 1554 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1555 | * 引数selectValue: |
Yajirushi | 0:3807ce385b2a | 1556 | * 1bit目: 加速度センサー単位系(0:m/s^2, 1:mg) |
Yajirushi | 0:3807ce385b2a | 1557 | * 2bit目: 角速度センサー単位系(0:deg/s, 1:rad/s) |
Yajirushi | 0:3807ce385b2a | 1558 | * 3bit目: オイラー角単位系(0:deg, 1:rad) |
Yajirushi | 0:3807ce385b2a | 1559 | * 5bit目: 温度単位系(0:摂氏, 1:華氏) |
Yajirushi | 0:3807ce385b2a | 1560 | * 7bit目: 出力設定(0:Windows, 1:Android) |
Yajirushi | 0:3807ce385b2a | 1561 | * returns: |
Yajirushi | 0:3807ce385b2a | 1562 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1563 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1564 | */ |
Yajirushi | 0:3807ce385b2a | 1565 | char BOARDC_BNO055::setUNIT_SEL(char selectValue){ |
Yajirushi | 0:3807ce385b2a | 1566 | if(ctrl->wr(0, BNO055P0_UNIT_SEL, selectValue) == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 1567 | |
Yajirushi | 0:3807ce385b2a | 1568 | scaleACC = ((selectValue & 0x01) == 0) ? 0.01f : 1.0f; |
Yajirushi | 0:3807ce385b2a | 1569 | scaleGYRO = ((selectValue & 0x02) == 0) ? 0.0625f : 0.001111111111f; |
Yajirushi | 0:3807ce385b2a | 1570 | scaleEuler = ((selectValue & 0x04) == 0) ? 0.0625f : 0.001111111111f; |
Yajirushi | 0:3807ce385b2a | 1571 | scaleTEMP = ((selectValue & 0x10) == 0) ? 1.0f : 2.0f; |
Yajirushi | 0:3807ce385b2a | 1572 | scaleLIA = scaleACC; |
Yajirushi | 0:3807ce385b2a | 1573 | scaleGV = scaleACC; |
Yajirushi | 0:3807ce385b2a | 1574 | |
Yajirushi | 0:3807ce385b2a | 1575 | return 1; |
Yajirushi | 0:3807ce385b2a | 1576 | } |
Yajirushi | 0:3807ce385b2a | 1577 | |
Yajirushi | 0:3807ce385b2a | 1578 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1579 | * システムの設定単位系(加速度センサー)を設定する |
Yajirushi | 0:3807ce385b2a | 1580 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1581 | * 引数isMeterPerSec2: |
Yajirushi | 0:3807ce385b2a | 1582 | * true: m/s^2 |
Yajirushi | 0:3807ce385b2a | 1583 | * false: mg |
Yajirushi | 0:3807ce385b2a | 1584 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1585 | * returns: |
Yajirushi | 0:3807ce385b2a | 1586 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1587 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1588 | */ |
Yajirushi | 0:3807ce385b2a | 1589 | char BOARDC_BNO055::setUNIT_AccUnit(bool isMeterPerSec2){ |
Yajirushi | 0:3807ce385b2a | 1590 | char val = getUNIT_SEL() & 0xFE; |
Yajirushi | 0:3807ce385b2a | 1591 | if(!isMeterPerSec2) val += 1; |
Yajirushi | 0:3807ce385b2a | 1592 | return setUNIT_SEL(val); |
Yajirushi | 0:3807ce385b2a | 1593 | } |
Yajirushi | 0:3807ce385b2a | 1594 | |
Yajirushi | 0:3807ce385b2a | 1595 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1596 | * システムの設定単位系(角速度センサー)を設定する |
Yajirushi | 0:3807ce385b2a | 1597 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1598 | * 引数isDps: |
Yajirushi | 0:3807ce385b2a | 1599 | * true: dps(Degrees/s) |
Yajirushi | 0:3807ce385b2a | 1600 | * false: rps(Radians/s) |
Yajirushi | 0:3807ce385b2a | 1601 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1602 | * returns: |
Yajirushi | 0:3807ce385b2a | 1603 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1604 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1605 | */ |
Yajirushi | 0:3807ce385b2a | 1606 | char BOARDC_BNO055::setUNIT_GyroUnit(bool isDps){ |
Yajirushi | 0:3807ce385b2a | 1607 | char val = getUNIT_SEL() & 0xFD; |
Yajirushi | 0:3807ce385b2a | 1608 | if(!isDps) val += 2; |
Yajirushi | 0:3807ce385b2a | 1609 | return setUNIT_SEL(val); |
Yajirushi | 0:3807ce385b2a | 1610 | } |
Yajirushi | 0:3807ce385b2a | 1611 | |
Yajirushi | 0:3807ce385b2a | 1612 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1613 | * システムの設定単位系(オイラー角)を設定する |
Yajirushi | 0:3807ce385b2a | 1614 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1615 | * 引数isDegrees: |
Yajirushi | 0:3807ce385b2a | 1616 | * true: Degrees |
Yajirushi | 0:3807ce385b2a | 1617 | * false: Radians |
Yajirushi | 0:3807ce385b2a | 1618 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1619 | * returns: |
Yajirushi | 0:3807ce385b2a | 1620 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1621 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1622 | */ |
Yajirushi | 0:3807ce385b2a | 1623 | char BOARDC_BNO055::setUNIT_EulerUnit(bool isDegrees){ |
Yajirushi | 0:3807ce385b2a | 1624 | char val = getUNIT_SEL() & 0xFB; |
Yajirushi | 0:3807ce385b2a | 1625 | if(!isDegrees) val += 4; |
Yajirushi | 0:3807ce385b2a | 1626 | return setUNIT_SEL(val); |
Yajirushi | 0:3807ce385b2a | 1627 | } |
Yajirushi | 0:3807ce385b2a | 1628 | |
Yajirushi | 0:3807ce385b2a | 1629 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1630 | * システムの設定単位系(温度)を設定する |
Yajirushi | 0:3807ce385b2a | 1631 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1632 | * 引数isCelsius: |
Yajirushi | 0:3807ce385b2a | 1633 | * true: 摂氏(Celsius) |
Yajirushi | 0:3807ce385b2a | 1634 | * false: 華氏(Fahrenheit) |
Yajirushi | 0:3807ce385b2a | 1635 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1636 | * returns: |
Yajirushi | 0:3807ce385b2a | 1637 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1638 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1639 | */ |
Yajirushi | 0:3807ce385b2a | 1640 | char BOARDC_BNO055::setUNIT_Temperature(bool isCelsius){ |
Yajirushi | 0:3807ce385b2a | 1641 | char val = getUNIT_SEL() & 0xEF; |
Yajirushi | 0:3807ce385b2a | 1642 | if(!isCelsius) val += 16; |
Yajirushi | 0:3807ce385b2a | 1643 | return setUNIT_SEL(val); |
Yajirushi | 0:3807ce385b2a | 1644 | } |
Yajirushi | 0:3807ce385b2a | 1645 | |
Yajirushi | 0:3807ce385b2a | 1646 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1647 | * システムの設定単位系(出力設定)を設定する |
Yajirushi | 0:3807ce385b2a | 1648 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1649 | * 引数ori_Android: |
Yajirushi | 0:3807ce385b2a | 1650 | * true: Android用(Pitch角:+180 to -180 反時計回り+) |
Yajirushi | 0:3807ce385b2a | 1651 | * false: Windows用(Pitch角:-180 to +180 時計回り+) |
Yajirushi | 0:3807ce385b2a | 1652 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1653 | * returns: |
Yajirushi | 0:3807ce385b2a | 1654 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1655 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1656 | */ |
Yajirushi | 0:3807ce385b2a | 1657 | char BOARDC_BNO055::setUNIT_OrientationMode(bool ori_Android){ |
Yajirushi | 0:3807ce385b2a | 1658 | char val = getUNIT_SEL() & 0xBF; |
Yajirushi | 0:3807ce385b2a | 1659 | if(!ori_Android) val += 64; |
Yajirushi | 0:3807ce385b2a | 1660 | return setUNIT_SEL(val); |
Yajirushi | 0:3807ce385b2a | 1661 | } |
Yajirushi | 0:3807ce385b2a | 1662 | |
Yajirushi | 0:3807ce385b2a | 1663 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1664 | * システムの動作モードを取得する |
Yajirushi | 0:3807ce385b2a | 1665 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1666 | * returns: |
Yajirushi | 0:3807ce385b2a | 1667 | * 1 ACCONLY(NO FUSION)加速度のみ |
Yajirushi | 0:3807ce385b2a | 1668 | * 2 MAGONLY(NO FUSION)地磁気のみ |
Yajirushi | 0:3807ce385b2a | 1669 | * 3 GYROONLY(NO FUSION)角速度のみ |
Yajirushi | 0:3807ce385b2a | 1670 | * 4 ACCMAG(NO FUSION)加速度と地磁気 |
Yajirushi | 0:3807ce385b2a | 1671 | * 5 ACCGYRO(NO FUSION)加速度と角速度 |
Yajirushi | 0:3807ce385b2a | 1672 | * 6 MAGGYRO(NO FUSION)地磁気と角速度 |
Yajirushi | 0:3807ce385b2a | 1673 | * 7 AMG(NO FUSION)加速度、地磁気、角速度 |
Yajirushi | 0:3807ce385b2a | 1674 | * 8 IMU |
Yajirushi | 0:3807ce385b2a | 1675 | * 9 COMPASS |
Yajirushi | 0:3807ce385b2a | 1676 | * 10 M4G |
Yajirushi | 0:3807ce385b2a | 1677 | * 11 NDOF_FMC_OFF |
Yajirushi | 0:3807ce385b2a | 1678 | * 12 NDOF |
Yajirushi | 0:3807ce385b2a | 1679 | */ |
Yajirushi | 0:3807ce385b2a | 1680 | char BOARDC_BNO055::getOperationMode(){ |
Yajirushi | 0:3807ce385b2a | 1681 | return ctrl->rr(0, BNO055P0_OPR_MODE); |
Yajirushi | 0:3807ce385b2a | 1682 | } |
Yajirushi | 0:3807ce385b2a | 1683 | |
Yajirushi | 0:3807ce385b2a | 1684 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1685 | * システムの動作モードを設定する |
Yajirushi | 0:3807ce385b2a | 1686 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1687 | * 引数modeValue: |
Yajirushi | 0:3807ce385b2a | 1688 | * 1 ACCONLY(NO FUSION)加速度のみ |
Yajirushi | 0:3807ce385b2a | 1689 | * 2 MAGONLY(NO FUSION)地磁気のみ |
Yajirushi | 0:3807ce385b2a | 1690 | * 3 GYROONLY(NO FUSION)角速度のみ |
Yajirushi | 0:3807ce385b2a | 1691 | * 4 ACCMAG(NO FUSION)加速度と地磁気 |
Yajirushi | 0:3807ce385b2a | 1692 | * 5 ACCGYRO(NO FUSION)加速度と角速度 |
Yajirushi | 0:3807ce385b2a | 1693 | * 6 MAGGYRO(NO FUSION)地磁気と角速度 |
Yajirushi | 0:3807ce385b2a | 1694 | * 7 AMG(NO FUSION)加速度、地磁気、角速度 |
Yajirushi | 0:3807ce385b2a | 1695 | * 8 IMU |
Yajirushi | 0:3807ce385b2a | 1696 | * 9 COMPASS |
Yajirushi | 0:3807ce385b2a | 1697 | * 10 M4G |
Yajirushi | 0:3807ce385b2a | 1698 | * 11 NDOF_FMC_OFF |
Yajirushi | 0:3807ce385b2a | 1699 | * 12 NDOF |
Yajirushi | 0:3807ce385b2a | 1700 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1701 | * returns: |
Yajirushi | 0:3807ce385b2a | 1702 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1703 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1704 | */ |
Yajirushi | 0:3807ce385b2a | 1705 | char BOARDC_BNO055::setOperationMode(char modeValue){ |
Yajirushi | 0:3807ce385b2a | 1706 | if(modeValue < 0 || modeValue > 12) modeValue = 7; |
Yajirushi | 0:3807ce385b2a | 1707 | return ctrl->wr(0, BNO055P0_OPR_MODE, modeValue); |
Yajirushi | 0:3807ce385b2a | 1708 | } |
Yajirushi | 0:3807ce385b2a | 1709 | |
Yajirushi | 0:3807ce385b2a | 1710 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1711 | * システムの動作モードを設定モードに設定する |
Yajirushi | 0:3807ce385b2a | 1712 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1713 | * returns: |
Yajirushi | 0:3807ce385b2a | 1714 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1715 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1716 | */ |
Yajirushi | 0:3807ce385b2a | 1717 | char BOARDC_BNO055::setOperation_CONFIG(){ |
Yajirushi | 0:3807ce385b2a | 1718 | return ctrl->wr(0, BNO055P0_OPR_MODE, 0); |
Yajirushi | 0:3807ce385b2a | 1719 | } |
Yajirushi | 0:3807ce385b2a | 1720 | |
Yajirushi | 0:3807ce385b2a | 1721 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1722 | * システムの動作モードを加速度モードに設定する |
Yajirushi | 0:3807ce385b2a | 1723 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1724 | * returns: |
Yajirushi | 0:3807ce385b2a | 1725 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1726 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1727 | */ |
Yajirushi | 0:3807ce385b2a | 1728 | char BOARDC_BNO055::setOperation_ACCONRY(){ |
Yajirushi | 0:3807ce385b2a | 1729 | return ctrl->wr(0, BNO055P0_OPR_MODE, 1); |
Yajirushi | 0:3807ce385b2a | 1730 | } |
Yajirushi | 0:3807ce385b2a | 1731 | |
Yajirushi | 0:3807ce385b2a | 1732 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1733 | * システムの動作モードを地磁気モードに設定する |
Yajirushi | 0:3807ce385b2a | 1734 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1735 | * returns: |
Yajirushi | 0:3807ce385b2a | 1736 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1737 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1738 | */ |
Yajirushi | 0:3807ce385b2a | 1739 | char BOARDC_BNO055::setOperation_MAGONRY(){ |
Yajirushi | 0:3807ce385b2a | 1740 | return ctrl->wr(0, BNO055P0_OPR_MODE, 2); |
Yajirushi | 0:3807ce385b2a | 1741 | } |
Yajirushi | 0:3807ce385b2a | 1742 | |
Yajirushi | 0:3807ce385b2a | 1743 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1744 | * システムの動作モードを角速度モードに設定する |
Yajirushi | 0:3807ce385b2a | 1745 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1746 | * returns: |
Yajirushi | 0:3807ce385b2a | 1747 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1748 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1749 | */ |
Yajirushi | 0:3807ce385b2a | 1750 | char BOARDC_BNO055::setOperation_GYROONRY(){ |
Yajirushi | 0:3807ce385b2a | 1751 | return ctrl->wr(0, BNO055P0_OPR_MODE, 3); |
Yajirushi | 0:3807ce385b2a | 1752 | } |
Yajirushi | 0:3807ce385b2a | 1753 | |
Yajirushi | 0:3807ce385b2a | 1754 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1755 | * システムの動作モードを加速度地磁気モードに設定する |
Yajirushi | 0:3807ce385b2a | 1756 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1757 | * returns: |
Yajirushi | 0:3807ce385b2a | 1758 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1759 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1760 | */ |
Yajirushi | 0:3807ce385b2a | 1761 | char BOARDC_BNO055::setOperation_ACCMAG(){ |
Yajirushi | 0:3807ce385b2a | 1762 | return ctrl->wr(0, BNO055P0_OPR_MODE, 4); |
Yajirushi | 0:3807ce385b2a | 1763 | } |
Yajirushi | 0:3807ce385b2a | 1764 | |
Yajirushi | 0:3807ce385b2a | 1765 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1766 | * システムの動作モードを加速度角速度モードに設定する |
Yajirushi | 0:3807ce385b2a | 1767 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1768 | * returns: |
Yajirushi | 0:3807ce385b2a | 1769 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1770 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1771 | */ |
Yajirushi | 0:3807ce385b2a | 1772 | char BOARDC_BNO055::setOperation_ACCGYRO(){ |
Yajirushi | 0:3807ce385b2a | 1773 | return ctrl->wr(0, BNO055P0_OPR_MODE, 5); |
Yajirushi | 0:3807ce385b2a | 1774 | } |
Yajirushi | 0:3807ce385b2a | 1775 | |
Yajirushi | 0:3807ce385b2a | 1776 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1777 | * システムの動作モードを地磁気角速度モードに設定する |
Yajirushi | 0:3807ce385b2a | 1778 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1779 | * returns: |
Yajirushi | 0:3807ce385b2a | 1780 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1781 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1782 | */ |
Yajirushi | 0:3807ce385b2a | 1783 | char BOARDC_BNO055::setOperation_MAGGYRO(){ |
Yajirushi | 0:3807ce385b2a | 1784 | return ctrl->wr(0, BNO055P0_OPR_MODE, 6); |
Yajirushi | 0:3807ce385b2a | 1785 | } |
Yajirushi | 0:3807ce385b2a | 1786 | |
Yajirushi | 0:3807ce385b2a | 1787 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1788 | * システムの動作モードをFusionなし9軸モードに設定する |
Yajirushi | 0:3807ce385b2a | 1789 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1790 | * returns: |
Yajirushi | 0:3807ce385b2a | 1791 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1792 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1793 | */ |
Yajirushi | 0:3807ce385b2a | 1794 | char BOARDC_BNO055::setOperation_AMG(){ |
Yajirushi | 0:3807ce385b2a | 1795 | return ctrl->wr(0, BNO055P0_OPR_MODE, 7); |
Yajirushi | 0:3807ce385b2a | 1796 | } |
Yajirushi | 0:3807ce385b2a | 1797 | |
Yajirushi | 0:3807ce385b2a | 1798 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1799 | * システムの動作モードを6軸(加速度、角速度)Fusionモードに設定する |
Yajirushi | 0:3807ce385b2a | 1800 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1801 | * returns: |
Yajirushi | 0:3807ce385b2a | 1802 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1803 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1804 | */ |
Yajirushi | 0:3807ce385b2a | 1805 | char BOARDC_BNO055::setOperation_Fusion_IMU(){ |
Yajirushi | 0:3807ce385b2a | 1806 | return ctrl->wr(0, BNO055P0_OPR_MODE, 8); |
Yajirushi | 0:3807ce385b2a | 1807 | } |
Yajirushi | 0:3807ce385b2a | 1808 | |
Yajirushi | 0:3807ce385b2a | 1809 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1810 | * システムの動作モードを6軸(加速度、地磁気)Fusionモード(相対系)に設定する |
Yajirushi | 0:3807ce385b2a | 1811 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1812 | * returns: |
Yajirushi | 0:3807ce385b2a | 1813 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1814 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1815 | */ |
Yajirushi | 0:3807ce385b2a | 1816 | char BOARDC_BNO055::setOperation_Fusion_COMPASS(){ |
Yajirushi | 0:3807ce385b2a | 1817 | return ctrl->wr(0, BNO055P0_OPR_MODE, 9); |
Yajirushi | 0:3807ce385b2a | 1818 | } |
Yajirushi | 0:3807ce385b2a | 1819 | |
Yajirushi | 0:3807ce385b2a | 1820 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1821 | * システムの動作モードを6軸(加速度、地磁気)Fusionモード(絶対系)に設定する |
Yajirushi | 0:3807ce385b2a | 1822 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1823 | * returns: |
Yajirushi | 0:3807ce385b2a | 1824 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1825 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1826 | */ |
Yajirushi | 0:3807ce385b2a | 1827 | char BOARDC_BNO055::setOperation_Fusion_M4G(){ |
Yajirushi | 0:3807ce385b2a | 1828 | return ctrl->wr(0, BNO055P0_OPR_MODE, 10); |
Yajirushi | 0:3807ce385b2a | 1829 | } |
Yajirushi | 0:3807ce385b2a | 1830 | |
Yajirushi | 0:3807ce385b2a | 1831 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1832 | * システムの動作モードをNDOFモード(地磁気短時間補正OFF)に設定する |
Yajirushi | 0:3807ce385b2a | 1833 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1834 | * returns: |
Yajirushi | 0:3807ce385b2a | 1835 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1836 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1837 | */ |
Yajirushi | 0:3807ce385b2a | 1838 | char BOARDC_BNO055::setOperation_Fusion_NDOF_FMC_OFF(){ |
Yajirushi | 0:3807ce385b2a | 1839 | return ctrl->wr(0, BNO055P0_OPR_MODE, 11); |
Yajirushi | 0:3807ce385b2a | 1840 | } |
Yajirushi | 0:3807ce385b2a | 1841 | |
Yajirushi | 0:3807ce385b2a | 1842 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1843 | * システムの動作モードをNDOFモードに設定する |
Yajirushi | 0:3807ce385b2a | 1844 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1845 | * returns: |
Yajirushi | 0:3807ce385b2a | 1846 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1847 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1848 | */ |
Yajirushi | 0:3807ce385b2a | 1849 | char BOARDC_BNO055::setOperation_Fusion_NDOF(){ |
Yajirushi | 0:3807ce385b2a | 1850 | return ctrl->wr(0, BNO055P0_OPR_MODE, 12); |
Yajirushi | 0:3807ce385b2a | 1851 | } |
Yajirushi | 0:3807ce385b2a | 1852 | |
Yajirushi | 0:3807ce385b2a | 1853 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1854 | * システムの電源モードを取得する |
Yajirushi | 0:3807ce385b2a | 1855 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1856 | * returns: |
Yajirushi | 0:3807ce385b2a | 1857 | * 0: Normal |
Yajirushi | 0:3807ce385b2a | 1858 | * 1: LowPower |
Yajirushi | 0:3807ce385b2a | 1859 | * 2: Suspend |
Yajirushi | 0:3807ce385b2a | 1860 | */ |
Yajirushi | 0:3807ce385b2a | 1861 | char BOARDC_BNO055::getPowerMode(){ |
Yajirushi | 0:3807ce385b2a | 1862 | return ctrl->rr(0, BNO055P0_PWR_MODE); |
Yajirushi | 0:3807ce385b2a | 1863 | } |
Yajirushi | 0:3807ce385b2a | 1864 | |
Yajirushi | 0:3807ce385b2a | 1865 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1866 | * システムの電源モードを設定する |
Yajirushi | 0:3807ce385b2a | 1867 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1868 | * modeValue: |
Yajirushi | 0:3807ce385b2a | 1869 | * 0: Normal |
Yajirushi | 0:3807ce385b2a | 1870 | * 1: LowPower |
Yajirushi | 0:3807ce385b2a | 1871 | * 2: Suspend |
Yajirushi | 0:3807ce385b2a | 1872 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1873 | * returns: |
Yajirushi | 0:3807ce385b2a | 1874 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1875 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1876 | */ |
Yajirushi | 0:3807ce385b2a | 1877 | char BOARDC_BNO055::setPowerMode(unsigned char modeValue){ |
Yajirushi | 0:3807ce385b2a | 1878 | if(modeValue > 2) return -1; |
Yajirushi | 0:3807ce385b2a | 1879 | return ctrl->wr(0, BNO055P0_PWR_MODE, modeValue); |
Yajirushi | 0:3807ce385b2a | 1880 | } |
Yajirushi | 0:3807ce385b2a | 1881 | |
Yajirushi | 0:3807ce385b2a | 1882 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1883 | * システムの電源モードを通常モードに設定する |
Yajirushi | 0:3807ce385b2a | 1884 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1885 | * returns: |
Yajirushi | 0:3807ce385b2a | 1886 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1887 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1888 | */ |
Yajirushi | 0:3807ce385b2a | 1889 | char BOARDC_BNO055::setPowerMode_Normal(){ |
Yajirushi | 0:3807ce385b2a | 1890 | return ctrl->wr(0, BNO055P0_PWR_MODE, 0); |
Yajirushi | 0:3807ce385b2a | 1891 | } |
Yajirushi | 0:3807ce385b2a | 1892 | |
Yajirushi | 0:3807ce385b2a | 1893 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1894 | * システムの電源モードを低消費電力モードに設定する |
Yajirushi | 0:3807ce385b2a | 1895 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1896 | * returns: |
Yajirushi | 0:3807ce385b2a | 1897 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1898 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1899 | */ |
Yajirushi | 0:3807ce385b2a | 1900 | char BOARDC_BNO055::setPowerMode_LowPower(){ |
Yajirushi | 0:3807ce385b2a | 1901 | return ctrl->wr(0, BNO055P0_PWR_MODE, 1); |
Yajirushi | 0:3807ce385b2a | 1902 | } |
Yajirushi | 0:3807ce385b2a | 1903 | |
Yajirushi | 0:3807ce385b2a | 1904 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1905 | * システムの電源モードをスリープモードに設定する |
Yajirushi | 0:3807ce385b2a | 1906 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1907 | * returns: |
Yajirushi | 0:3807ce385b2a | 1908 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1909 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1910 | */ |
Yajirushi | 0:3807ce385b2a | 1911 | char BOARDC_BNO055::setPowerMode_Suspend(){ |
Yajirushi | 0:3807ce385b2a | 1912 | return ctrl->wr(0, BNO055P0_PWR_MODE, 2); |
Yajirushi | 0:3807ce385b2a | 1913 | } |
Yajirushi | 0:3807ce385b2a | 1914 | |
Yajirushi | 0:3807ce385b2a | 1915 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1916 | * システムのトリガー設定を変更する |
Yajirushi | 0:3807ce385b2a | 1917 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1918 | * returns: |
Yajirushi | 0:3807ce385b2a | 1919 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1920 | * 0 変更なし |
Yajirushi | 0:3807ce385b2a | 1921 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1922 | */ |
Yajirushi | 0:3807ce385b2a | 1923 | char BOARDC_BNO055::setSysTrigger(char regVal){ |
Yajirushi | 0:3807ce385b2a | 1924 | clkExt = regVal >> 7; |
Yajirushi | 0:3807ce385b2a | 1925 | return ctrl->wr(0, BNO055P0_SYS_TRIGGER, regVal & 0xE1); |
Yajirushi | 0:3807ce385b2a | 1926 | } |
Yajirushi | 0:3807ce385b2a | 1927 | |
Yajirushi | 0:3807ce385b2a | 1928 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1929 | * システムのクロック発振元を設定する |
Yajirushi | 0:3807ce385b2a | 1930 | * 外部を指定する場合は、設定前にXIN,XOUTが発振子に結線されている必要がある |
Yajirushi | 0:3807ce385b2a | 1931 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1932 | * returns: |
Yajirushi | 0:3807ce385b2a | 1933 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1934 | * 0 変更なし |
Yajirushi | 0:3807ce385b2a | 1935 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1936 | */ |
Yajirushi | 0:3807ce385b2a | 1937 | char BOARDC_BNO055::setSys_ExternalCrystal(bool isExternal){ |
Yajirushi | 0:3807ce385b2a | 1938 | if(clkExt == isExternal) return 0; |
Yajirushi | 0:3807ce385b2a | 1939 | |
Yajirushi | 0:3807ce385b2a | 1940 | clkExt = isExternal; |
Yajirushi | 0:3807ce385b2a | 1941 | return ctrl->wr(0, BNO055P0_SYS_TRIGGER, (clkExt) ? 0x80 : 0x00); |
Yajirushi | 0:3807ce385b2a | 1942 | } |
Yajirushi | 0:3807ce385b2a | 1943 | |
Yajirushi | 0:3807ce385b2a | 1944 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1945 | * システムの割り込み発生をすべてリセットする |
Yajirushi | 0:3807ce385b2a | 1946 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1947 | * returns: |
Yajirushi | 0:3807ce385b2a | 1948 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1949 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1950 | */ |
Yajirushi | 0:3807ce385b2a | 1951 | char BOARDC_BNO055::resetInterrupt(){ |
Yajirushi | 0:3807ce385b2a | 1952 | return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x40); |
Yajirushi | 0:3807ce385b2a | 1953 | } |
Yajirushi | 0:3807ce385b2a | 1954 | |
Yajirushi | 0:3807ce385b2a | 1955 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1956 | * システムをリセットする |
Yajirushi | 0:3807ce385b2a | 1957 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1958 | * returns: |
Yajirushi | 0:3807ce385b2a | 1959 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1960 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1961 | */ |
Yajirushi | 0:3807ce385b2a | 1962 | char BOARDC_BNO055::soft_reset(){ |
Yajirushi | 0:3807ce385b2a | 1963 | return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x20); |
Yajirushi | 0:3807ce385b2a | 1964 | } |
Yajirushi | 0:3807ce385b2a | 1965 | |
Yajirushi | 0:3807ce385b2a | 1966 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1967 | * セルフテストを実行する |
Yajirushi | 0:3807ce385b2a | 1968 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1969 | * returns: |
Yajirushi | 0:3807ce385b2a | 1970 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1971 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1972 | */ |
Yajirushi | 0:3807ce385b2a | 1973 | char BOARDC_BNO055::execSelfTest(){ |
Yajirushi | 0:3807ce385b2a | 1974 | return ctrl->wr(0, BNO055P0_SYS_TRIGGER, ((clkExt) ? 0x80 : 0x00) | 0x01); |
Yajirushi | 0:3807ce385b2a | 1975 | } |
Yajirushi | 0:3807ce385b2a | 1976 | |
Yajirushi | 0:3807ce385b2a | 1977 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1978 | * システムの温度計測に使用するセンサーを取得する |
Yajirushi | 0:3807ce385b2a | 1979 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1980 | * returns: |
Yajirushi | 0:3807ce385b2a | 1981 | * 0 温度計測に加速度センサーを使用している |
Yajirushi | 0:3807ce385b2a | 1982 | * 1 温度計測に角速度センサーを使用している |
Yajirushi | 0:3807ce385b2a | 1983 | */ |
Yajirushi | 0:3807ce385b2a | 1984 | char BOARDC_BNO055::getTempSource(){ |
Yajirushi | 0:3807ce385b2a | 1985 | return ctrl->rr(0, BNO055P0_TEMP_SOURCE); |
Yajirushi | 0:3807ce385b2a | 1986 | } |
Yajirushi | 0:3807ce385b2a | 1987 | |
Yajirushi | 0:3807ce385b2a | 1988 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 1989 | * システムの温度計測に使用するセンサーを選択する |
Yajirushi | 0:3807ce385b2a | 1990 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1991 | * 引数Accelerometer: |
Yajirushi | 0:3807ce385b2a | 1992 | * true:加速度センサーを温度計測に使用する |
Yajirushi | 0:3807ce385b2a | 1993 | * false:角速度センサーを温度計測に使用する |
Yajirushi | 0:3807ce385b2a | 1994 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 1995 | * returns: |
Yajirushi | 0:3807ce385b2a | 1996 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 1997 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 1998 | */ |
Yajirushi | 0:3807ce385b2a | 1999 | char BOARDC_BNO055::setTempSource(bool Accelerometer){ |
Yajirushi | 0:3807ce385b2a | 2000 | return ctrl->wr(0, BNO055P0_TEMP_SOURCE, (Accelerometer) ? 0 : 1); |
Yajirushi | 0:3807ce385b2a | 2001 | } |
Yajirushi | 0:3807ce385b2a | 2002 | |
Yajirushi | 0:3807ce385b2a | 2003 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2004 | * センサー出力軸交換情報の取得 |
Yajirushi | 0:3807ce385b2a | 2005 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2006 | * returns: |
Yajirushi | 0:3807ce385b2a | 2007 | * AXIS_MAP_CONFIGの設定情報:BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2008 | */ |
Yajirushi | 0:3807ce385b2a | 2009 | char BOARDC_BNO055::getAxisMapConfig(){ |
Yajirushi | 0:3807ce385b2a | 2010 | char ret = ctrl->rr(0, BNO055P0_AXIS_MAP_CONFIG); |
Yajirushi | 0:3807ce385b2a | 2011 | if(axisRemap != ret) axisRemap = ret; |
Yajirushi | 0:3807ce385b2a | 2012 | return axisRemap; |
Yajirushi | 0:3807ce385b2a | 2013 | } |
Yajirushi | 0:3807ce385b2a | 2014 | |
Yajirushi | 0:3807ce385b2a | 2015 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2016 | * センサー出力軸交換の設定 |
Yajirushi | 0:3807ce385b2a | 2017 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2018 | * 引数val: |
Yajirushi | 0:3807ce385b2a | 2019 | * X,Y,Zの出力交換の情報 |
Yajirushi | 0:3807ce385b2a | 2020 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2021 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2022 | * returns: |
Yajirushi | 0:3807ce385b2a | 2023 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2024 | * 0 変更なし |
Yajirushi | 0:3807ce385b2a | 2025 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2026 | */ |
Yajirushi | 0:3807ce385b2a | 2027 | char BOARDC_BNO055::setAxisMapConfig(char val){ |
Yajirushi | 0:3807ce385b2a | 2028 | if(axisRemap == (val & 0x3F)) return 0; |
Yajirushi | 0:3807ce385b2a | 2029 | else axisRemap = (val & 0x3F); |
Yajirushi | 0:3807ce385b2a | 2030 | return ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2031 | } |
Yajirushi | 0:3807ce385b2a | 2032 | |
Yajirushi | 0:3807ce385b2a | 2033 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2034 | * センサー出力軸符号情報の取得 |
Yajirushi | 0:3807ce385b2a | 2035 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2036 | * returns: |
Yajirushi | 0:3807ce385b2a | 2037 | * AXIS_MAP_SIGNの設定情報:BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2038 | */ |
Yajirushi | 0:3807ce385b2a | 2039 | char BOARDC_BNO055::getAxisMapSign(){ |
Yajirushi | 0:3807ce385b2a | 2040 | char ret = ctrl->rr(0, BNO055P0_AXIS_MAP_SIGN); |
Yajirushi | 0:3807ce385b2a | 2041 | if(axisSign != ret) axisSign = ret; |
Yajirushi | 0:3807ce385b2a | 2042 | return axisSign; |
Yajirushi | 0:3807ce385b2a | 2043 | } |
Yajirushi | 0:3807ce385b2a | 2044 | |
Yajirushi | 0:3807ce385b2a | 2045 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2046 | * センサー出力軸符号の設定 |
Yajirushi | 0:3807ce385b2a | 2047 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2048 | * 引数val: |
Yajirushi | 0:3807ce385b2a | 2049 | * X,Y,Zの出力符号の情報 |
Yajirushi | 0:3807ce385b2a | 2050 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2051 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2052 | * returns: |
Yajirushi | 0:3807ce385b2a | 2053 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2054 | * 0 変更なし |
Yajirushi | 0:3807ce385b2a | 2055 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2056 | */ |
Yajirushi | 0:3807ce385b2a | 2057 | char BOARDC_BNO055::setAxisMapSign(char val){ |
Yajirushi | 0:3807ce385b2a | 2058 | if(axisSign == (val & 0x07)) return 0; |
Yajirushi | 0:3807ce385b2a | 2059 | else axisSign = (val & 0x07); |
Yajirushi | 0:3807ce385b2a | 2060 | return ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2061 | } |
Yajirushi | 0:3807ce385b2a | 2062 | |
Yajirushi | 0:3807ce385b2a | 2063 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2064 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側左上] |
Yajirushi | 0:3807ce385b2a | 2065 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2066 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2067 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2068 | * returns: |
Yajirushi | 0:3807ce385b2a | 2069 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2070 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2071 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2072 | */ |
Yajirushi | 0:3807ce385b2a | 2073 | char BOARDC_BNO055::setAxisRemap_topview_topleft(){ |
Yajirushi | 0:3807ce385b2a | 2074 | axisRemap = 0x21; |
Yajirushi | 0:3807ce385b2a | 2075 | axisSign = 0x04; |
Yajirushi | 0:3807ce385b2a | 2076 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2077 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2078 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2079 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2080 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2081 | return 1; |
Yajirushi | 0:3807ce385b2a | 2082 | } |
Yajirushi | 0:3807ce385b2a | 2083 | |
Yajirushi | 0:3807ce385b2a | 2084 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2085 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側右上] |
Yajirushi | 0:3807ce385b2a | 2086 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2087 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2088 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2089 | * returns: |
Yajirushi | 0:3807ce385b2a | 2090 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2091 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2092 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2093 | */ |
Yajirushi | 0:3807ce385b2a | 2094 | char BOARDC_BNO055::setAxisRemap_topview_topright(){ |
Yajirushi | 0:3807ce385b2a | 2095 | axisRemap = 0x24; |
Yajirushi | 0:3807ce385b2a | 2096 | axisSign = 0x00; |
Yajirushi | 0:3807ce385b2a | 2097 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2098 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2099 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2100 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2101 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2102 | return 1; |
Yajirushi | 0:3807ce385b2a | 2103 | } |
Yajirushi | 0:3807ce385b2a | 2104 | |
Yajirushi | 0:3807ce385b2a | 2105 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2106 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側左下] |
Yajirushi | 0:3807ce385b2a | 2107 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2108 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2109 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2110 | * returns: |
Yajirushi | 0:3807ce385b2a | 2111 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2112 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2113 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2114 | */ |
Yajirushi | 0:3807ce385b2a | 2115 | char BOARDC_BNO055::setAxisRemap_topview_bottomleft(){ |
Yajirushi | 0:3807ce385b2a | 2116 | axisRemap = 0x24; |
Yajirushi | 0:3807ce385b2a | 2117 | axisSign = 0x06; |
Yajirushi | 0:3807ce385b2a | 2118 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2119 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2120 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2121 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2122 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2123 | return 1; |
Yajirushi | 0:3807ce385b2a | 2124 | } |
Yajirushi | 0:3807ce385b2a | 2125 | |
Yajirushi | 0:3807ce385b2a | 2126 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2127 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側右下] |
Yajirushi | 0:3807ce385b2a | 2128 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2129 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2130 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2131 | * returns: |
Yajirushi | 0:3807ce385b2a | 2132 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2133 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2134 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2135 | */ |
Yajirushi | 0:3807ce385b2a | 2136 | char BOARDC_BNO055::setAxisRemap_topview_bottomright(){ |
Yajirushi | 0:3807ce385b2a | 2137 | axisRemap = 0x21; |
Yajirushi | 0:3807ce385b2a | 2138 | axisSign = 0x02; |
Yajirushi | 0:3807ce385b2a | 2139 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2140 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2141 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2142 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2143 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2144 | return 1; |
Yajirushi | 0:3807ce385b2a | 2145 | } |
Yajirushi | 0:3807ce385b2a | 2146 | |
Yajirushi | 0:3807ce385b2a | 2147 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2148 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側左上] |
Yajirushi | 0:3807ce385b2a | 2149 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2150 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2151 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2152 | * returns: |
Yajirushi | 0:3807ce385b2a | 2153 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2154 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2155 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2156 | */ |
Yajirushi | 0:3807ce385b2a | 2157 | char BOARDC_BNO055::setAxisRemap_bottomview_topleft(){ |
Yajirushi | 0:3807ce385b2a | 2158 | axisRemap = 0x24; |
Yajirushi | 0:3807ce385b2a | 2159 | axisSign = 0x03; |
Yajirushi | 0:3807ce385b2a | 2160 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2161 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2162 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2163 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2164 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2165 | return 1; |
Yajirushi | 0:3807ce385b2a | 2166 | } |
Yajirushi | 0:3807ce385b2a | 2167 | |
Yajirushi | 0:3807ce385b2a | 2168 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2169 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側右上] |
Yajirushi | 0:3807ce385b2a | 2170 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2171 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2172 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2173 | * returns: |
Yajirushi | 0:3807ce385b2a | 2174 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2175 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2176 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2177 | */ |
Yajirushi | 0:3807ce385b2a | 2178 | char BOARDC_BNO055::setAxisRemap_bottomview_topright(){ |
Yajirushi | 0:3807ce385b2a | 2179 | axisRemap = 0x21; |
Yajirushi | 0:3807ce385b2a | 2180 | axisSign = 0x01; |
Yajirushi | 0:3807ce385b2a | 2181 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2182 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2183 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2184 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2185 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2186 | return 1; |
Yajirushi | 0:3807ce385b2a | 2187 | } |
Yajirushi | 0:3807ce385b2a | 2188 | |
Yajirushi | 0:3807ce385b2a | 2189 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2190 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側左下] |
Yajirushi | 0:3807ce385b2a | 2191 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2192 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2193 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2194 | * returns: |
Yajirushi | 0:3807ce385b2a | 2195 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2196 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2197 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2198 | */ |
Yajirushi | 0:3807ce385b2a | 2199 | char BOARDC_BNO055::setAxisRemap_bottomview_bottomleft(){ |
Yajirushi | 0:3807ce385b2a | 2200 | axisRemap = 0x21; |
Yajirushi | 0:3807ce385b2a | 2201 | axisSign = 0x07; |
Yajirushi | 0:3807ce385b2a | 2202 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2203 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2204 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2205 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2206 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2207 | return 1; |
Yajirushi | 0:3807ce385b2a | 2208 | } |
Yajirushi | 0:3807ce385b2a | 2209 | |
Yajirushi | 0:3807ce385b2a | 2210 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2211 | * センサー出力軸の符号と交換を直感的に設定する[1pinが表側から見たとき裏側右下] |
Yajirushi | 0:3807ce385b2a | 2212 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2213 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2214 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2215 | * returns: |
Yajirushi | 0:3807ce385b2a | 2216 | * -1 AXIS_MAP_CONFIG設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2217 | * -2 AXIS_MAP_SIGN設定の失敗 |
Yajirushi | 0:3807ce385b2a | 2218 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2219 | */ |
Yajirushi | 0:3807ce385b2a | 2220 | char BOARDC_BNO055::setAxisRemap_bottomview_bottomright(){ |
Yajirushi | 0:3807ce385b2a | 2221 | axisRemap = 0x24; |
Yajirushi | 0:3807ce385b2a | 2222 | axisSign = 0x05; |
Yajirushi | 0:3807ce385b2a | 2223 | char ret = 0; |
Yajirushi | 0:3807ce385b2a | 2224 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_CONFIG, axisRemap); |
Yajirushi | 0:3807ce385b2a | 2225 | if(ret == -1) return -1; |
Yajirushi | 0:3807ce385b2a | 2226 | ret = ctrl->wr(0, BNO055P0_AXIS_MAP_SIGN, axisSign); |
Yajirushi | 0:3807ce385b2a | 2227 | if(ret == -1) return -2; |
Yajirushi | 0:3807ce385b2a | 2228 | return 1; |
Yajirushi | 0:3807ce385b2a | 2229 | } |
Yajirushi | 0:3807ce385b2a | 2230 | |
Yajirushi | 0:3807ce385b2a | 2231 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2232 | * センサー出力軸の符号と交換の情報を直感的な形で取得する |
Yajirushi | 0:3807ce385b2a | 2233 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2234 | * (BNO055データシート [3.4 Axis remap] を参照 |
Yajirushi | 0:3807ce385b2a | 2235 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2236 | * returns: |
Yajirushi | 0:3807ce385b2a | 2237 | * 0 [P0]表側から見たとき、BNO055の1pinが表側左上にある状態 |
Yajirushi | 0:3807ce385b2a | 2238 | * 1 [P1]表側から見たとき、BNO055の1pinが表側右上にある状態 |
Yajirushi | 0:3807ce385b2a | 2239 | * 2 [P2]表側から見たとき、BNO055の1pinが表側左下にある状態 |
Yajirushi | 0:3807ce385b2a | 2240 | * 3 [P3]表側から見たとき、BNO055の1pinが表側右下にある状態 |
Yajirushi | 0:3807ce385b2a | 2241 | * 4 [P4]表側から見たとき、BNO055の1pinが裏側左上にある状態 |
Yajirushi | 0:3807ce385b2a | 2242 | * 5 [P5]表側から見たとき、BNO055の1pinが裏側右上にある状態 |
Yajirushi | 0:3807ce385b2a | 2243 | * 6 [P6]表側から見たとき、BNO055の1pinが裏側左下にある状態 |
Yajirushi | 0:3807ce385b2a | 2244 | * 7 [P7]表側から見たとき、BNO055の1pinが裏側右下にある状態 |
Yajirushi | 0:3807ce385b2a | 2245 | * -1 それ以外の設定 |
Yajirushi | 0:3807ce385b2a | 2246 | */ |
Yajirushi | 0:3807ce385b2a | 2247 | char BOARDC_BNO055::getAxisRemap_type(){ |
Yajirushi | 0:3807ce385b2a | 2248 | getAxisMapConfig(); //axisRemapに最新の値を格納 |
Yajirushi | 0:3807ce385b2a | 2249 | getAxisMapSign(); //axisSignに最新の値を格納 |
Yajirushi | 0:3807ce385b2a | 2250 | |
Yajirushi | 0:3807ce385b2a | 2251 | if(axisRemap == 0x21){ |
Yajirushi | 0:3807ce385b2a | 2252 | switch(axisSign){ |
Yajirushi | 0:3807ce385b2a | 2253 | case 0x04: |
Yajirushi | 0:3807ce385b2a | 2254 | return 0; |
Yajirushi | 0:3807ce385b2a | 2255 | case 0x02: |
Yajirushi | 0:3807ce385b2a | 2256 | return 3; |
Yajirushi | 0:3807ce385b2a | 2257 | case 0x01: |
Yajirushi | 0:3807ce385b2a | 2258 | return 5; |
Yajirushi | 0:3807ce385b2a | 2259 | case 0x07: |
Yajirushi | 0:3807ce385b2a | 2260 | return 6; |
Yajirushi | 0:3807ce385b2a | 2261 | } |
Yajirushi | 0:3807ce385b2a | 2262 | }else if(axisRemap == 0x24){ |
Yajirushi | 0:3807ce385b2a | 2263 | switch(axisSign){ |
Yajirushi | 0:3807ce385b2a | 2264 | case 0x00: |
Yajirushi | 0:3807ce385b2a | 2265 | return 1; |
Yajirushi | 0:3807ce385b2a | 2266 | case 0x06: |
Yajirushi | 0:3807ce385b2a | 2267 | return 2; |
Yajirushi | 0:3807ce385b2a | 2268 | case 0x03: |
Yajirushi | 0:3807ce385b2a | 2269 | return 4; |
Yajirushi | 0:3807ce385b2a | 2270 | case 0x05: |
Yajirushi | 0:3807ce385b2a | 2271 | return 7; |
Yajirushi | 0:3807ce385b2a | 2272 | } |
Yajirushi | 0:3807ce385b2a | 2273 | } |
Yajirushi | 0:3807ce385b2a | 2274 | |
Yajirushi | 0:3807ce385b2a | 2275 | return -1; |
Yajirushi | 0:3807ce385b2a | 2276 | } |
Yajirushi | 0:3807ce385b2a | 2277 | |
Yajirushi | 0:3807ce385b2a | 2278 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2279 | * 加速度センサーの補正値を取得する |
Yajirushi | 0:3807ce385b2a | 2280 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2281 | * &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2282 | * &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2283 | * &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2284 | */ |
Yajirushi | 0:3807ce385b2a | 2285 | void BOARDC_BNO055::getAccOffsetAll(float &offsetX, float &offsetY, float &offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2286 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 2287 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 2288 | ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 2289 | |
Yajirushi | 0:3807ce385b2a | 2290 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2291 | short offY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2292 | short offZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 2293 | offsetX = (1.0f * offX) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2294 | offsetY = (1.0f * offY) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2295 | offsetZ = (1.0f * offZ) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2296 | } |
Yajirushi | 0:3807ce385b2a | 2297 | |
Yajirushi | 0:3807ce385b2a | 2298 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2299 | * 加速度センサーの補正値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2300 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2301 | * returns: |
Yajirushi | 0:3807ce385b2a | 2302 | * X軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2303 | */ |
Yajirushi | 0:3807ce385b2a | 2304 | float BOARDC_BNO055::getAccOffsetX(){ |
Yajirushi | 0:3807ce385b2a | 2305 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2306 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2307 | ctrl->rrc(0, BNO055P0_ACC_OFFSET_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2308 | |
Yajirushi | 0:3807ce385b2a | 2309 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2310 | return (1.0f * offX) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2311 | } |
Yajirushi | 0:3807ce385b2a | 2312 | |
Yajirushi | 0:3807ce385b2a | 2313 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2314 | * 加速度センサーの補正値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2315 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2316 | * returns: |
Yajirushi | 0:3807ce385b2a | 2317 | * Y軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2318 | */ |
Yajirushi | 0:3807ce385b2a | 2319 | float BOARDC_BNO055::getAccOffsetY(){ |
Yajirushi | 0:3807ce385b2a | 2320 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2321 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2322 | ctrl->rrc(0, BNO055P0_ACC_OFFSET_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2323 | |
Yajirushi | 0:3807ce385b2a | 2324 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2325 | return (1.0f * offX) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2326 | } |
Yajirushi | 0:3807ce385b2a | 2327 | |
Yajirushi | 0:3807ce385b2a | 2328 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2329 | * 加速度センサーの補正値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2330 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2331 | * returns: |
Yajirushi | 0:3807ce385b2a | 2332 | * Z軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2333 | */ |
Yajirushi | 0:3807ce385b2a | 2334 | float BOARDC_BNO055::getAccOffsetZ(){ |
Yajirushi | 0:3807ce385b2a | 2335 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2336 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2337 | ctrl->rrc(0, BNO055P0_ACC_OFFSET_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2338 | |
Yajirushi | 0:3807ce385b2a | 2339 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2340 | return (1.0f * offX) * scaleACC; |
Yajirushi | 0:3807ce385b2a | 2341 | } |
Yajirushi | 0:3807ce385b2a | 2342 | |
Yajirushi | 0:3807ce385b2a | 2343 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2344 | * 加速度センサーの補正値を設定する |
Yajirushi | 0:3807ce385b2a | 2345 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2346 | * offsetX: スケール乗算済みのX軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2347 | * offsetY: スケール乗算済みのY軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2348 | * offsetZ: スケール乗算済みのZ軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2349 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2350 | * returns: |
Yajirushi | 0:3807ce385b2a | 2351 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2352 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2353 | */ |
Yajirushi | 0:3807ce385b2a | 2354 | char BOARDC_BNO055::setAccOffsetAll(float offsetX, float offsetY, float offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2355 | short offX = (short)((offsetX / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2356 | short offY = (short)((offsetY / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2357 | short offZ = (short)((offsetZ / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2358 | char msg[6]; |
Yajirushi | 0:3807ce385b2a | 2359 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2360 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2361 | msg[2] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2362 | msg[3] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2363 | msg[4] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2364 | msg[5] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2365 | |
Yajirushi | 0:3807ce385b2a | 2366 | //連続6byte書き込み |
Yajirushi | 0:3807ce385b2a | 2367 | return ctrl->wrc(0, BNO055P0_ACC_OFFSET_X_LSB, msg, 6); |
Yajirushi | 0:3807ce385b2a | 2368 | } |
Yajirushi | 0:3807ce385b2a | 2369 | |
Yajirushi | 0:3807ce385b2a | 2370 | char BOARDC_BNO055::setAccOffsetX(float offset){ |
Yajirushi | 0:3807ce385b2a | 2371 | short offX = (short)((offset / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2372 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2373 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2374 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2375 | |
Yajirushi | 0:3807ce385b2a | 2376 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2377 | return ctrl->wrc(0, BNO055P0_ACC_OFFSET_X_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2378 | } |
Yajirushi | 0:3807ce385b2a | 2379 | |
Yajirushi | 0:3807ce385b2a | 2380 | char BOARDC_BNO055::setAccOffsetY(float offset){ |
Yajirushi | 0:3807ce385b2a | 2381 | short offY = (short)((offset / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2382 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2383 | msg[0] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2384 | msg[1] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2385 | |
Yajirushi | 0:3807ce385b2a | 2386 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2387 | return ctrl->wrc(0, BNO055P0_ACC_OFFSET_Y_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2388 | } |
Yajirushi | 0:3807ce385b2a | 2389 | |
Yajirushi | 0:3807ce385b2a | 2390 | char BOARDC_BNO055::setAccOffsetZ(float offset){ |
Yajirushi | 0:3807ce385b2a | 2391 | short offZ = (short)((offset / scaleACC) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2392 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2393 | msg[0] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2394 | msg[1] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2395 | |
Yajirushi | 0:3807ce385b2a | 2396 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2397 | return ctrl->wrc(0, BNO055P0_ACC_OFFSET_Z_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2398 | } |
Yajirushi | 0:3807ce385b2a | 2399 | |
Yajirushi | 0:3807ce385b2a | 2400 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2401 | * 地磁気センサーの補正値を取得する |
Yajirushi | 0:3807ce385b2a | 2402 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2403 | * &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2404 | * &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2405 | * &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2406 | */ |
Yajirushi | 0:3807ce385b2a | 2407 | void BOARDC_BNO055::getMagOffsetAll(float &offsetX, float &offsetY, float &offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2408 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 2409 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 2410 | ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 2411 | |
Yajirushi | 0:3807ce385b2a | 2412 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2413 | short offY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2414 | short offZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 2415 | offsetX = (1.0f * offX) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2416 | offsetY = (1.0f * offY) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2417 | offsetZ = (1.0f * offZ) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2418 | } |
Yajirushi | 0:3807ce385b2a | 2419 | |
Yajirushi | 0:3807ce385b2a | 2420 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2421 | * 地磁気センサーの補正値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2422 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2423 | * returns: |
Yajirushi | 0:3807ce385b2a | 2424 | * X軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2425 | */ |
Yajirushi | 0:3807ce385b2a | 2426 | float BOARDC_BNO055::getMagOffsetX(){ |
Yajirushi | 0:3807ce385b2a | 2427 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2428 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2429 | ctrl->rrc(0, BNO055P0_MAG_OFFSET_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2430 | |
Yajirushi | 0:3807ce385b2a | 2431 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2432 | return (1.0f * offX) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2433 | } |
Yajirushi | 0:3807ce385b2a | 2434 | |
Yajirushi | 0:3807ce385b2a | 2435 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2436 | * 地磁気センサーの補正値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2437 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2438 | * returns: |
Yajirushi | 0:3807ce385b2a | 2439 | * Y軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2440 | */ |
Yajirushi | 0:3807ce385b2a | 2441 | float BOARDC_BNO055::getMagOffsetY(){ |
Yajirushi | 0:3807ce385b2a | 2442 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2443 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2444 | ctrl->rrc(0, BNO055P0_MAG_OFFSET_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2445 | |
Yajirushi | 0:3807ce385b2a | 2446 | short offY = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2447 | return (1.0f * offY) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2448 | } |
Yajirushi | 0:3807ce385b2a | 2449 | |
Yajirushi | 0:3807ce385b2a | 2450 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2451 | * 地磁気センサーの補正値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2452 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2453 | * returns: |
Yajirushi | 0:3807ce385b2a | 2454 | * Z軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2455 | */ |
Yajirushi | 0:3807ce385b2a | 2456 | float BOARDC_BNO055::getMagOffsetZ(){ |
Yajirushi | 0:3807ce385b2a | 2457 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2458 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2459 | ctrl->rrc(0, BNO055P0_MAG_OFFSET_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2460 | |
Yajirushi | 0:3807ce385b2a | 2461 | short offZ = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2462 | return (1.0f * offZ) * scaleMAG; |
Yajirushi | 0:3807ce385b2a | 2463 | } |
Yajirushi | 0:3807ce385b2a | 2464 | |
Yajirushi | 0:3807ce385b2a | 2465 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2466 | * 地磁気センサーの補正値を設定する |
Yajirushi | 0:3807ce385b2a | 2467 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2468 | * offsetX: スケール乗算済みのX軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2469 | * offsetY: スケール乗算済みのY軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2470 | * offsetZ: スケール乗算済みのZ軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2471 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2472 | * returns: |
Yajirushi | 0:3807ce385b2a | 2473 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2474 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2475 | */ |
Yajirushi | 0:3807ce385b2a | 2476 | char BOARDC_BNO055::setMagOffsetAll(float offsetX, float offsetY, float offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2477 | short offX = (short)((offsetX / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2478 | short offY = (short)((offsetY / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2479 | short offZ = (short)((offsetZ / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2480 | char msg[6]; |
Yajirushi | 0:3807ce385b2a | 2481 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2482 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2483 | msg[2] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2484 | msg[3] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2485 | msg[4] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2486 | msg[5] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2487 | |
Yajirushi | 0:3807ce385b2a | 2488 | //連続6byte書き込み |
Yajirushi | 0:3807ce385b2a | 2489 | return ctrl->wrc(0, BNO055P0_MAG_OFFSET_X_LSB, msg, 6); |
Yajirushi | 0:3807ce385b2a | 2490 | } |
Yajirushi | 0:3807ce385b2a | 2491 | |
Yajirushi | 0:3807ce385b2a | 2492 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2493 | * 地磁気センサーの補正値(X軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2494 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2495 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2496 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2497 | * returns: |
Yajirushi | 0:3807ce385b2a | 2498 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2499 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2500 | */ |
Yajirushi | 0:3807ce385b2a | 2501 | char BOARDC_BNO055::setMagOffsetX(float offset){ |
Yajirushi | 0:3807ce385b2a | 2502 | short offX = (short)((offset / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2503 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2504 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2505 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2506 | |
Yajirushi | 0:3807ce385b2a | 2507 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2508 | return ctrl->wrc(0, BNO055P0_MAG_OFFSET_X_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2509 | } |
Yajirushi | 0:3807ce385b2a | 2510 | |
Yajirushi | 0:3807ce385b2a | 2511 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2512 | * 地磁気センサーの補正値(Y軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2513 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2514 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2515 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2516 | * returns: |
Yajirushi | 0:3807ce385b2a | 2517 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2518 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2519 | */ |
Yajirushi | 0:3807ce385b2a | 2520 | char BOARDC_BNO055::setMagOffsetY(float offset){ |
Yajirushi | 0:3807ce385b2a | 2521 | short offY = (short)((offset / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2522 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2523 | msg[0] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2524 | msg[1] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2525 | |
Yajirushi | 0:3807ce385b2a | 2526 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2527 | return ctrl->wrc(0, BNO055P0_MAG_OFFSET_Y_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2528 | } |
Yajirushi | 0:3807ce385b2a | 2529 | |
Yajirushi | 0:3807ce385b2a | 2530 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2531 | * 地磁気センサーの補正値(Z軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2532 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2533 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2534 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2535 | * returns: |
Yajirushi | 0:3807ce385b2a | 2536 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2537 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2538 | */ |
Yajirushi | 0:3807ce385b2a | 2539 | char BOARDC_BNO055::setMagOffsetZ(float offset){ |
Yajirushi | 0:3807ce385b2a | 2540 | short offZ = (short)((offset / scaleMAG) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2541 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2542 | msg[0] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2543 | msg[1] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2544 | |
Yajirushi | 0:3807ce385b2a | 2545 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2546 | return ctrl->wrc(0, BNO055P0_MAG_OFFSET_Z_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2547 | } |
Yajirushi | 0:3807ce385b2a | 2548 | |
Yajirushi | 0:3807ce385b2a | 2549 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2550 | * 角速度センサーの補正値を取得する |
Yajirushi | 0:3807ce385b2a | 2551 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2552 | * &offsetX: アドレス参照引数:関数実行後、この変数にX軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2553 | * &offsetY: アドレス参照引数:関数実行後、この変数にY軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2554 | * &offsetZ: アドレス参照引数:関数実行後、この変数にZ軸補正値が格納される |
Yajirushi | 0:3807ce385b2a | 2555 | */ |
Yajirushi | 0:3807ce385b2a | 2556 | void BOARDC_BNO055::getGyroOffsetAll(float &offsetX, float &offsetY, float &offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2557 | //連続6byte読み取り |
Yajirushi | 0:3807ce385b2a | 2558 | char rsv[6]; |
Yajirushi | 0:3807ce385b2a | 2559 | ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 6); |
Yajirushi | 0:3807ce385b2a | 2560 | |
Yajirushi | 0:3807ce385b2a | 2561 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2562 | short offY = (rsv[3] << 8) | rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2563 | short offZ = (rsv[5] << 8) | rsv[4]; |
Yajirushi | 0:3807ce385b2a | 2564 | offsetX = (1.0f * offX) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2565 | offsetY = (1.0f * offY) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2566 | offsetZ = (1.0f * offZ) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2567 | } |
Yajirushi | 0:3807ce385b2a | 2568 | |
Yajirushi | 0:3807ce385b2a | 2569 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2570 | * 角速度センサーの補正値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2571 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2572 | * returns: |
Yajirushi | 0:3807ce385b2a | 2573 | * X軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2574 | */ |
Yajirushi | 0:3807ce385b2a | 2575 | float BOARDC_BNO055::getGyroOffsetX(){ |
Yajirushi | 0:3807ce385b2a | 2576 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2577 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2578 | ctrl->rrc(0, BNO055P0_GYR_OFFSET_X_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2579 | |
Yajirushi | 0:3807ce385b2a | 2580 | short offX = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2581 | return (1.0f * offX) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2582 | } |
Yajirushi | 0:3807ce385b2a | 2583 | |
Yajirushi | 0:3807ce385b2a | 2584 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2585 | * 角速度センサーの補正値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2586 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2587 | * returns: |
Yajirushi | 0:3807ce385b2a | 2588 | * Y軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2589 | */ |
Yajirushi | 0:3807ce385b2a | 2590 | float BOARDC_BNO055::getGyroOffsetY(){ |
Yajirushi | 0:3807ce385b2a | 2591 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2592 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2593 | ctrl->rrc(0, BNO055P0_GYR_OFFSET_Y_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2594 | |
Yajirushi | 0:3807ce385b2a | 2595 | short offY = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2596 | return (1.0f * offY) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2597 | } |
Yajirushi | 0:3807ce385b2a | 2598 | |
Yajirushi | 0:3807ce385b2a | 2599 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2600 | * 角速度センサーの補正値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 2601 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2602 | * returns: |
Yajirushi | 0:3807ce385b2a | 2603 | * Z軸補正値(スケール乗算済み) |
Yajirushi | 0:3807ce385b2a | 2604 | */ |
Yajirushi | 0:3807ce385b2a | 2605 | float BOARDC_BNO055::getGyroOffsetZ(){ |
Yajirushi | 0:3807ce385b2a | 2606 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2607 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2608 | ctrl->rrc(0, BNO055P0_GYR_OFFSET_Z_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2609 | |
Yajirushi | 0:3807ce385b2a | 2610 | short offZ = (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2611 | return (1.0f * offZ) * scaleGYRO; |
Yajirushi | 0:3807ce385b2a | 2612 | } |
Yajirushi | 0:3807ce385b2a | 2613 | |
Yajirushi | 0:3807ce385b2a | 2614 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2615 | * 角速度センサーの補正値を設定する |
Yajirushi | 0:3807ce385b2a | 2616 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2617 | * offsetX: スケール乗算済みのX軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2618 | * offsetY: スケール乗算済みのY軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2619 | * offsetZ: スケール乗算済みのZ軸の補正値 |
Yajirushi | 0:3807ce385b2a | 2620 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2621 | * returns: |
Yajirushi | 0:3807ce385b2a | 2622 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2623 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2624 | */ |
Yajirushi | 0:3807ce385b2a | 2625 | char BOARDC_BNO055::setGyroOffsetAll(float offsetX, float offsetY, float offsetZ){ |
Yajirushi | 0:3807ce385b2a | 2626 | short offX = (short)((offsetX / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2627 | short offY = (short)((offsetY / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2628 | short offZ = (short)((offsetZ / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2629 | char msg[6]; |
Yajirushi | 0:3807ce385b2a | 2630 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2631 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2632 | msg[2] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2633 | msg[3] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2634 | msg[4] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2635 | msg[5] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2636 | |
Yajirushi | 0:3807ce385b2a | 2637 | //連続6byte書き込み |
Yajirushi | 0:3807ce385b2a | 2638 | return ctrl->wrc(0, BNO055P0_GYR_OFFSET_X_LSB, msg, 6); |
Yajirushi | 0:3807ce385b2a | 2639 | } |
Yajirushi | 0:3807ce385b2a | 2640 | |
Yajirushi | 0:3807ce385b2a | 2641 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2642 | * 角速度センサーの補正値(X軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2643 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2644 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2645 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2646 | * returns: |
Yajirushi | 0:3807ce385b2a | 2647 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2648 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2649 | */ |
Yajirushi | 0:3807ce385b2a | 2650 | char BOARDC_BNO055::setGyroOffsetX(float offset){ |
Yajirushi | 0:3807ce385b2a | 2651 | short offX = (short)((offset / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2652 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2653 | msg[0] = offX & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2654 | msg[1] = offX >> 8; |
Yajirushi | 0:3807ce385b2a | 2655 | |
Yajirushi | 0:3807ce385b2a | 2656 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2657 | return ctrl->wrc(0, BNO055P0_GYR_OFFSET_X_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2658 | } |
Yajirushi | 0:3807ce385b2a | 2659 | |
Yajirushi | 0:3807ce385b2a | 2660 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2661 | * 角速度センサーの補正値(Y軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2662 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2663 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2664 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2665 | * returns: |
Yajirushi | 0:3807ce385b2a | 2666 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2667 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2668 | */ |
Yajirushi | 0:3807ce385b2a | 2669 | char BOARDC_BNO055::setGyroOffsetY(float offset){ |
Yajirushi | 0:3807ce385b2a | 2670 | short offY = (short)((offset / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2671 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2672 | msg[0] = offY & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2673 | msg[1] = offY >> 8; |
Yajirushi | 0:3807ce385b2a | 2674 | |
Yajirushi | 0:3807ce385b2a | 2675 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2676 | return ctrl->wrc(0, BNO055P0_GYR_OFFSET_Y_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2677 | } |
Yajirushi | 0:3807ce385b2a | 2678 | |
Yajirushi | 0:3807ce385b2a | 2679 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2680 | * 角速度センサーの補正値(Z軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 2681 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2682 | * offset: スケール乗算済みの補正値 |
Yajirushi | 0:3807ce385b2a | 2683 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2684 | * returns: |
Yajirushi | 0:3807ce385b2a | 2685 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2686 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2687 | */ |
Yajirushi | 0:3807ce385b2a | 2688 | char BOARDC_BNO055::setGyroOffsetZ(float offset){ |
Yajirushi | 0:3807ce385b2a | 2689 | short offZ = (short)((offset / scaleGYRO) + 0.5f); |
Yajirushi | 0:3807ce385b2a | 2690 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2691 | msg[0] = offZ & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2692 | msg[1] = offZ >> 8; |
Yajirushi | 0:3807ce385b2a | 2693 | |
Yajirushi | 0:3807ce385b2a | 2694 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2695 | return ctrl->wrc(0, BNO055P0_GYR_OFFSET_Z_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2696 | } |
Yajirushi | 0:3807ce385b2a | 2697 | |
Yajirushi | 0:3807ce385b2a | 2698 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2699 | * 加速度センサーのデータ出力範囲[単位:LSB]を取得する |
Yajirushi | 0:3807ce385b2a | 2700 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2701 | * returns: |
Yajirushi | 0:3807ce385b2a | 2702 | * +-データ計測範囲[単位:LSB] |
Yajirushi | 0:3807ce385b2a | 2703 | */ |
Yajirushi | 0:3807ce385b2a | 2704 | short BOARDC_BNO055::getAccRadius(){ |
Yajirushi | 0:3807ce385b2a | 2705 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2706 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2707 | ctrl->rrc(0, BNO055P0_ACC_RADIUS_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2708 | |
Yajirushi | 0:3807ce385b2a | 2709 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2710 | } |
Yajirushi | 0:3807ce385b2a | 2711 | |
Yajirushi | 0:3807ce385b2a | 2712 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2713 | * 加速度センサーのデータ出力範囲[単位:LSB]を設定する |
Yajirushi | 0:3807ce385b2a | 2714 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2715 | * 引数r: データ計測範囲の最大値および最低値(-r から +r の範囲が計測範囲) |
Yajirushi | 0:3807ce385b2a | 2716 | * 最大値は1000[LSB] |
Yajirushi | 0:3807ce385b2a | 2717 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2718 | * returns: |
Yajirushi | 0:3807ce385b2a | 2719 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2720 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2721 | */ |
Yajirushi | 0:3807ce385b2a | 2722 | char BOARDC_BNO055::setAccRadius(short LSB){ |
Yajirushi | 0:3807ce385b2a | 2723 | if(LSB > 1000) LSB = 1000; |
Yajirushi | 0:3807ce385b2a | 2724 | |
Yajirushi | 0:3807ce385b2a | 2725 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2726 | msg[0] = LSB & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2727 | msg[1] = LSB >> 8; |
Yajirushi | 0:3807ce385b2a | 2728 | |
Yajirushi | 0:3807ce385b2a | 2729 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2730 | return ctrl->wrc(0, BNO055P0_ACC_RADIUS_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2731 | } |
Yajirushi | 0:3807ce385b2a | 2732 | |
Yajirushi | 0:3807ce385b2a | 2733 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2734 | * 地磁気センサーのデータ出力範囲[単位:LSB]を取得する |
Yajirushi | 0:3807ce385b2a | 2735 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2736 | * returns: |
Yajirushi | 0:3807ce385b2a | 2737 | * +-データ計測範囲 |
Yajirushi | 0:3807ce385b2a | 2738 | */ |
Yajirushi | 0:3807ce385b2a | 2739 | short BOARDC_BNO055::getMagRadius(){ |
Yajirushi | 0:3807ce385b2a | 2740 | //連続2byte読み取り |
Yajirushi | 0:3807ce385b2a | 2741 | char rsv[2]; |
Yajirushi | 0:3807ce385b2a | 2742 | ctrl->rrc(0, BNO055P0_MAG_RADIUS_LSB, rsv, 2); |
Yajirushi | 0:3807ce385b2a | 2743 | |
Yajirushi | 0:3807ce385b2a | 2744 | return (rsv[1] << 8) | rsv[0]; |
Yajirushi | 0:3807ce385b2a | 2745 | } |
Yajirushi | 0:3807ce385b2a | 2746 | |
Yajirushi | 0:3807ce385b2a | 2747 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2748 | * 加速度センサーのデータ出力範囲[単位:LSB]を設定する |
Yajirushi | 0:3807ce385b2a | 2749 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2750 | * 引数r: データ計測範囲の最大値および最低値(-r から +r の範囲が計測範囲) |
Yajirushi | 0:3807ce385b2a | 2751 | * 最大値は960[LSB] |
Yajirushi | 0:3807ce385b2a | 2752 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2753 | * returns: |
Yajirushi | 0:3807ce385b2a | 2754 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2755 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2756 | */ |
Yajirushi | 0:3807ce385b2a | 2757 | char BOARDC_BNO055::setMagRadius(short LSB){ |
Yajirushi | 0:3807ce385b2a | 2758 | if(LSB > 960) LSB = 960; |
Yajirushi | 0:3807ce385b2a | 2759 | |
Yajirushi | 0:3807ce385b2a | 2760 | char msg[2]; |
Yajirushi | 0:3807ce385b2a | 2761 | msg[0] = LSB & 0xFF; |
Yajirushi | 0:3807ce385b2a | 2762 | msg[1] = LSB >> 8; |
Yajirushi | 0:3807ce385b2a | 2763 | |
Yajirushi | 0:3807ce385b2a | 2764 | //連続2byte書き込み |
Yajirushi | 0:3807ce385b2a | 2765 | return ctrl->wrc(0, BNO055P0_MAG_RADIUS_LSB, msg, 2); |
Yajirushi | 0:3807ce385b2a | 2766 | } |
Yajirushi | 0:3807ce385b2a | 2767 | |
Yajirushi | 0:3807ce385b2a | 2768 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2769 | * 加速度センサー設定のレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 2770 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2771 | * returns: |
Yajirushi | 0:3807ce385b2a | 2772 | * 加速度センサー設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2773 | */ |
Yajirushi | 0:3807ce385b2a | 2774 | char BOARDC_BNO055::getAccConfig(){ |
Yajirushi | 0:3807ce385b2a | 2775 | return ctrl->rr(1, BNO055P1_ACC_CONFIG); |
Yajirushi | 0:3807ce385b2a | 2776 | } |
Yajirushi | 0:3807ce385b2a | 2777 | |
Yajirushi | 0:3807ce385b2a | 2778 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2779 | * 加速度センサー設定のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2780 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2781 | * 引数regVal: 加速度センサー設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2782 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2783 | * returns: |
Yajirushi | 0:3807ce385b2a | 2784 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2785 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2786 | */ |
Yajirushi | 0:3807ce385b2a | 2787 | char BOARDC_BNO055::setAccConfig(char regVal){ |
Yajirushi | 0:3807ce385b2a | 2788 | return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2789 | } |
Yajirushi | 0:3807ce385b2a | 2790 | |
Yajirushi | 0:3807ce385b2a | 2791 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2792 | * 加速度センサー設定のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2793 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2794 | * 引数gRange: 加速度センサーの計測範囲 |
Yajirushi | 0:3807ce385b2a | 2795 | * 引数bandWidth: 加速度センサー出力レート(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2796 | * 引数powMode: 加速度センサー電源設定(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2797 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2798 | * returns: |
Yajirushi | 0:3807ce385b2a | 2799 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2800 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2801 | */ |
Yajirushi | 0:3807ce385b2a | 2802 | char BOARDC_BNO055::setAccConfig(char gRange, char bandWidth, char powMode){ |
Yajirushi | 0:3807ce385b2a | 2803 | char regVal = (powMode << 5) | (bandWidth << 2) | gRange; |
Yajirushi | 0:3807ce385b2a | 2804 | return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2805 | } |
Yajirushi | 0:3807ce385b2a | 2806 | |
Yajirushi | 0:3807ce385b2a | 2807 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2808 | * 加速度センサーの計測範囲を設定する |
Yajirushi | 0:3807ce385b2a | 2809 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2810 | * 引数G: 加速度センサーの計測範囲(+-2, 4, 8, 16Gのいずれか) |
Yajirushi | 0:3807ce385b2a | 2811 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2812 | * returns: |
Yajirushi | 0:3807ce385b2a | 2813 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2814 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2815 | */ |
Yajirushi | 0:3807ce385b2a | 2816 | char BOARDC_BNO055::setAccRange(unsigned char G){ |
Yajirushi | 0:3807ce385b2a | 2817 | char val = 0; |
Yajirushi | 0:3807ce385b2a | 2818 | switch(G){ |
Yajirushi | 0:3807ce385b2a | 2819 | case 2: |
Yajirushi | 0:3807ce385b2a | 2820 | val = 0x00; |
Yajirushi | 0:3807ce385b2a | 2821 | break; |
Yajirushi | 0:3807ce385b2a | 2822 | case 4: |
Yajirushi | 0:3807ce385b2a | 2823 | val = 0x01; |
Yajirushi | 0:3807ce385b2a | 2824 | break; |
Yajirushi | 0:3807ce385b2a | 2825 | case 8: |
Yajirushi | 0:3807ce385b2a | 2826 | val = 0x02; |
Yajirushi | 0:3807ce385b2a | 2827 | break; |
Yajirushi | 0:3807ce385b2a | 2828 | case 16: |
Yajirushi | 0:3807ce385b2a | 2829 | val = 0x03; |
Yajirushi | 0:3807ce385b2a | 2830 | break; |
Yajirushi | 0:3807ce385b2a | 2831 | default: |
Yajirushi | 0:3807ce385b2a | 2832 | val = 0x02; |
Yajirushi | 0:3807ce385b2a | 2833 | } |
Yajirushi | 0:3807ce385b2a | 2834 | |
Yajirushi | 0:3807ce385b2a | 2835 | char regVal = ctrl->rr(1, BNO055P1_ACC_CONFIG); |
Yajirushi | 0:3807ce385b2a | 2836 | regVal = (regVal & 0xFC) | val; |
Yajirushi | 0:3807ce385b2a | 2837 | |
Yajirushi | 0:3807ce385b2a | 2838 | return ctrl->wr(1, BNO055P1_ACC_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2839 | } |
Yajirushi | 0:3807ce385b2a | 2840 | |
Yajirushi | 0:3807ce385b2a | 2841 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2842 | * 地磁気センサー設定のレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 2843 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2844 | * returns: |
Yajirushi | 0:3807ce385b2a | 2845 | * 地磁気センサー設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2846 | */ |
Yajirushi | 0:3807ce385b2a | 2847 | char BOARDC_BNO055::getMagConfig(){ |
Yajirushi | 0:3807ce385b2a | 2848 | return ctrl->rr(1, BNO055P1_MAG_CONFIG); |
Yajirushi | 0:3807ce385b2a | 2849 | } |
Yajirushi | 0:3807ce385b2a | 2850 | |
Yajirushi | 0:3807ce385b2a | 2851 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2852 | * 地磁気センサー設定のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2853 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2854 | * 引数regVal: 地磁気センサー設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2855 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2856 | * returns: |
Yajirushi | 0:3807ce385b2a | 2857 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2858 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2859 | */ |
Yajirushi | 0:3807ce385b2a | 2860 | char BOARDC_BNO055::setMagConfig(char regVal){ |
Yajirushi | 0:3807ce385b2a | 2861 | return ctrl->wr(1, BNO055P1_MAG_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2862 | } |
Yajirushi | 0:3807ce385b2a | 2863 | |
Yajirushi | 0:3807ce385b2a | 2864 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2865 | * 地磁気センサー設定のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2866 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2867 | * 引数rate: 地磁気センサー出力レート(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2868 | * 引数oprMode: 地磁気センサー出力モード(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2869 | * 引数powMode: 地磁気センサー電源設定(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2870 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2871 | * returns: |
Yajirushi | 0:3807ce385b2a | 2872 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2873 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2874 | */ |
Yajirushi | 0:3807ce385b2a | 2875 | char BOARDC_BNO055::setMagConfig(char rate, char oprMode, char powMode){ |
Yajirushi | 0:3807ce385b2a | 2876 | char regVal = powMode << 5 | oprMode << 3 | rate; |
Yajirushi | 0:3807ce385b2a | 2877 | return ctrl->wr(1, BNO055P1_MAG_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2878 | } |
Yajirushi | 0:3807ce385b2a | 2879 | |
Yajirushi | 0:3807ce385b2a | 2880 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2881 | * 角速度センサー設定(0)のレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 2882 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2883 | * returns: |
Yajirushi | 0:3807ce385b2a | 2884 | * 角速度センサー設定(0)のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2885 | */ |
Yajirushi | 0:3807ce385b2a | 2886 | char BOARDC_BNO055::getGyroConfig_0(){ |
Yajirushi | 0:3807ce385b2a | 2887 | return ctrl->rr(1, BNO055P1_GYR_CONFIG_0); |
Yajirushi | 0:3807ce385b2a | 2888 | } |
Yajirushi | 0:3807ce385b2a | 2889 | |
Yajirushi | 0:3807ce385b2a | 2890 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2891 | * 角速度センサー設定(0)のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2892 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2893 | * 引数regVal: 角速度センサー設定(0)のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2894 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2895 | * returns: |
Yajirushi | 0:3807ce385b2a | 2896 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2897 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2898 | */ |
Yajirushi | 0:3807ce385b2a | 2899 | char BOARDC_BNO055::setGyroConfig_0(char regVal){ |
Yajirushi | 0:3807ce385b2a | 2900 | return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal); |
Yajirushi | 0:3807ce385b2a | 2901 | } |
Yajirushi | 0:3807ce385b2a | 2902 | |
Yajirushi | 0:3807ce385b2a | 2903 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2904 | * 角速度センサー設定(0)のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2905 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2906 | * 引数range: 角速度センサーの計測範囲(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2907 | * 引数bandWidth: 角速度センサー出力レート(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2908 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2909 | * returns: |
Yajirushi | 0:3807ce385b2a | 2910 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2911 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2912 | */ |
Yajirushi | 0:3807ce385b2a | 2913 | char BOARDC_BNO055::setGyroConfig_0(char range, char bandWidth){ |
Yajirushi | 0:3807ce385b2a | 2914 | char regVal = bandWidth << 3 | range; |
Yajirushi | 0:3807ce385b2a | 2915 | return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal); |
Yajirushi | 0:3807ce385b2a | 2916 | } |
Yajirushi | 0:3807ce385b2a | 2917 | |
Yajirushi | 0:3807ce385b2a | 2918 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2919 | * 角速度センサー設定(1)のレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 2920 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2921 | * returns: |
Yajirushi | 0:3807ce385b2a | 2922 | * 角速度センサー設定(1)のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2923 | */ |
Yajirushi | 0:3807ce385b2a | 2924 | char BOARDC_BNO055::getGyroConfig_1(){ |
Yajirushi | 0:3807ce385b2a | 2925 | return ctrl->rr(1, BNO055P1_GYR_CONFIG_0); |
Yajirushi | 0:3807ce385b2a | 2926 | } |
Yajirushi | 0:3807ce385b2a | 2927 | |
Yajirushi | 0:3807ce385b2a | 2928 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2929 | * 角速度センサー設定(1)のレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2930 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2931 | * 引数powMode: 角速度センサーの電源設定(Fusion時は自動設定) |
Yajirushi | 0:3807ce385b2a | 2932 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2933 | * returns: |
Yajirushi | 0:3807ce385b2a | 2934 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2935 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2936 | */ |
Yajirushi | 0:3807ce385b2a | 2937 | char BOARDC_BNO055::setGyroConfig_1(char powMode){ |
Yajirushi | 0:3807ce385b2a | 2938 | return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, powMode & 0x07); |
Yajirushi | 0:3807ce385b2a | 2939 | } |
Yajirushi | 0:3807ce385b2a | 2940 | |
Yajirushi | 0:3807ce385b2a | 2941 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2942 | * 角速度センサーの計測範囲を設定する |
Yajirushi | 0:3807ce385b2a | 2943 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2944 | * 引数G: 角速度センサーの計測範囲(2000, 1000, 500, 250, 125dpsのいずれか) |
Yajirushi | 0:3807ce385b2a | 2945 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2946 | * returns: |
Yajirushi | 0:3807ce385b2a | 2947 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2948 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2949 | */ |
Yajirushi | 0:3807ce385b2a | 2950 | char BOARDC_BNO055::setGyroRange(unsigned short dps){ |
Yajirushi | 0:3807ce385b2a | 2951 | char val = 0; |
Yajirushi | 0:3807ce385b2a | 2952 | switch(dps){ |
Yajirushi | 0:3807ce385b2a | 2953 | case 2000: |
Yajirushi | 0:3807ce385b2a | 2954 | val = 0; |
Yajirushi | 0:3807ce385b2a | 2955 | break; |
Yajirushi | 0:3807ce385b2a | 2956 | case 1000: |
Yajirushi | 0:3807ce385b2a | 2957 | val = 1; |
Yajirushi | 0:3807ce385b2a | 2958 | break; |
Yajirushi | 0:3807ce385b2a | 2959 | case 500: |
Yajirushi | 0:3807ce385b2a | 2960 | val = 2; |
Yajirushi | 0:3807ce385b2a | 2961 | break; |
Yajirushi | 0:3807ce385b2a | 2962 | case 250: |
Yajirushi | 0:3807ce385b2a | 2963 | val = 3; |
Yajirushi | 0:3807ce385b2a | 2964 | break; |
Yajirushi | 0:3807ce385b2a | 2965 | case 125: |
Yajirushi | 0:3807ce385b2a | 2966 | val = 4; |
Yajirushi | 0:3807ce385b2a | 2967 | break; |
Yajirushi | 0:3807ce385b2a | 2968 | default: |
Yajirushi | 0:3807ce385b2a | 2969 | val = 1; |
Yajirushi | 0:3807ce385b2a | 2970 | } |
Yajirushi | 0:3807ce385b2a | 2971 | |
Yajirushi | 0:3807ce385b2a | 2972 | char regVal = ctrl->rr(1, BNO055P1_GYR_CONFIG_0); |
Yajirushi | 0:3807ce385b2a | 2973 | regVal = (regVal & 0xF8) + val; |
Yajirushi | 0:3807ce385b2a | 2974 | |
Yajirushi | 0:3807ce385b2a | 2975 | return ctrl->wr(1, BNO055P1_GYR_CONFIG_0, regVal); |
Yajirushi | 0:3807ce385b2a | 2976 | } |
Yajirushi | 0:3807ce385b2a | 2977 | |
Yajirushi | 0:3807ce385b2a | 2978 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2979 | * 加速度センサーのスリープモードのレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 2980 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2981 | * returns: |
Yajirushi | 0:3807ce385b2a | 2982 | * 加速度センサーのスリープモードのレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2983 | */ |
Yajirushi | 0:3807ce385b2a | 2984 | char BOARDC_BNO055::getAccSleepConfig(){ |
Yajirushi | 0:3807ce385b2a | 2985 | return ctrl->rr(1, BNO055P1_ACC_SLEEP_CONFIG); |
Yajirushi | 0:3807ce385b2a | 2986 | } |
Yajirushi | 0:3807ce385b2a | 2987 | |
Yajirushi | 0:3807ce385b2a | 2988 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 2989 | * 加速度センサーのスリープモードのレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 2990 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2991 | * 引数regVal: 加速度センサーのスリープモードのレジスタ値 |
Yajirushi | 0:3807ce385b2a | 2992 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 2993 | * returns: |
Yajirushi | 0:3807ce385b2a | 2994 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 2995 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 2996 | */ |
Yajirushi | 0:3807ce385b2a | 2997 | char BOARDC_BNO055::setAccSleepConfig(char regVal){ |
Yajirushi | 0:3807ce385b2a | 2998 | return ctrl->wr(1, BNO055P1_ACC_SLEEP_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 2999 | } |
Yajirushi | 0:3807ce385b2a | 3000 | |
Yajirushi | 0:3807ce385b2a | 3001 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3002 | * 加速度センサーのスリープモードのレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 3003 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3004 | * 引数duration: 加速度センサーのスリープモードの持続閾値 |
Yajirushi | 0:3807ce385b2a | 3005 | * 引数mode: 加速度センサーのスリープモード選択 |
Yajirushi | 0:3807ce385b2a | 3006 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3007 | * returns: |
Yajirushi | 0:3807ce385b2a | 3008 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3009 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3010 | */ |
Yajirushi | 0:3807ce385b2a | 3011 | char BOARDC_BNO055::setAccSleepConfig(char duration, char mode){ |
Yajirushi | 0:3807ce385b2a | 3012 | char regVal = duration << 1 | mode; |
Yajirushi | 0:3807ce385b2a | 3013 | return ctrl->wr(1, BNO055P1_ACC_SLEEP_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 3014 | } |
Yajirushi | 0:3807ce385b2a | 3015 | |
Yajirushi | 0:3807ce385b2a | 3016 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3017 | * 角速度センサーのスリープモードのレジスタ値を取得する |
Yajirushi | 0:3807ce385b2a | 3018 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3019 | * returns: |
Yajirushi | 0:3807ce385b2a | 3020 | * 角速度センサーのスリープモードのレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3021 | */ |
Yajirushi | 0:3807ce385b2a | 3022 | char BOARDC_BNO055::getGyroSleepConfig(){ |
Yajirushi | 0:3807ce385b2a | 3023 | return ctrl->rr(1, BNO055P1_GYR_SLEEP_CONFIG); |
Yajirushi | 0:3807ce385b2a | 3024 | } |
Yajirushi | 0:3807ce385b2a | 3025 | |
Yajirushi | 0:3807ce385b2a | 3026 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3027 | * 角速度センサーのスリープモードのレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 3028 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3029 | * 引数regVal: 角速度センサーのスリープモードのレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3030 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3031 | * returns: |
Yajirushi | 0:3807ce385b2a | 3032 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3033 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3034 | */ |
Yajirushi | 0:3807ce385b2a | 3035 | char BOARDC_BNO055::setGyroSleepConfig(char regVal){ |
Yajirushi | 0:3807ce385b2a | 3036 | return ctrl->wr(1, BNO055P1_GYR_SLEEP_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 3037 | } |
Yajirushi | 0:3807ce385b2a | 3038 | |
Yajirushi | 0:3807ce385b2a | 3039 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3040 | * 角速度センサーのスリープモードのレジスタ値を設定する |
Yajirushi | 0:3807ce385b2a | 3041 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3042 | * 引数duration: 角速度センサーの自動スリープモードの持続閾値 |
Yajirushi | 0:3807ce385b2a | 3043 | * 引数mode: 角速度センサーのスリープモードの持続閾値 |
Yajirushi | 0:3807ce385b2a | 3044 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3045 | * returns: |
Yajirushi | 0:3807ce385b2a | 3046 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3047 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3048 | */ |
Yajirushi | 0:3807ce385b2a | 3049 | char BOARDC_BNO055::setGyroSleepConfig(char autoSleepDuration, char duration){ |
Yajirushi | 0:3807ce385b2a | 3050 | char regVal = autoSleepDuration << 3 | duration; |
Yajirushi | 0:3807ce385b2a | 3051 | return ctrl->wr(1, BNO055P1_GYR_SLEEP_CONFIG, regVal); |
Yajirushi | 0:3807ce385b2a | 3052 | } |
Yajirushi | 0:3807ce385b2a | 3053 | |
Yajirushi | 0:3807ce385b2a | 3054 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3055 | * 各センサーの割り込みフラグ発生のINTピン出力許可設定を取得する |
Yajirushi | 0:3807ce385b2a | 3056 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3057 | * returns: |
Yajirushi | 0:3807ce385b2a | 3058 | * 各センサーの割り込みフラグ有効無効設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3059 | * 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3060 | * 3bit目 角速度センサーHighRate割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3061 | * 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3062 | * 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3063 | * 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3064 | */ |
Yajirushi | 0:3807ce385b2a | 3065 | char BOARDC_BNO055::getInterruptMask(){ |
Yajirushi | 0:3807ce385b2a | 3066 | return ctrl->rr(1, BNO055P1_INT_MSK); |
Yajirushi | 0:3807ce385b2a | 3067 | } |
Yajirushi | 0:3807ce385b2a | 3068 | |
Yajirushi | 0:3807ce385b2a | 3069 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3070 | * 各センサーの割り込みフラグ発生のINTピン出力許可設定を設定する |
Yajirushi | 0:3807ce385b2a | 3071 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3072 | * 引数mask: 各センサーの割り込みフラグ有効無効設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3073 | * 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3074 | * 3bit目 角速度センサーHighRate割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3075 | * 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3076 | * 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3077 | * 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:許可) |
Yajirushi | 0:3807ce385b2a | 3078 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3079 | * returns: |
Yajirushi | 0:3807ce385b2a | 3080 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3081 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3082 | */ |
Yajirushi | 0:3807ce385b2a | 3083 | char BOARDC_BNO055::setInterruptMask(char mask){ |
Yajirushi | 0:3807ce385b2a | 3084 | return ctrl->wr(1, BNO055P1_INT_MSK, mask); |
Yajirushi | 0:3807ce385b2a | 3085 | } |
Yajirushi | 0:3807ce385b2a | 3086 | |
Yajirushi | 0:3807ce385b2a | 3087 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3088 | * 各センサーの割り込みフラグ有効無効設定を取得する |
Yajirushi | 0:3807ce385b2a | 3089 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3090 | * returns: |
Yajirushi | 0:3807ce385b2a | 3091 | * 各センサーの割り込みフラグ有効無効設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3092 | * 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3093 | * 3bit目 角速度センサーHighRate割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3094 | * 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3095 | * 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3096 | * 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3097 | */ |
Yajirushi | 0:3807ce385b2a | 3098 | char BOARDC_BNO055::getInterruptEnable(){ |
Yajirushi | 0:3807ce385b2a | 3099 | return ctrl->rr(1, BNO055P1_INT_EN); |
Yajirushi | 0:3807ce385b2a | 3100 | } |
Yajirushi | 0:3807ce385b2a | 3101 | |
Yajirushi | 0:3807ce385b2a | 3102 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3103 | * 各センサーの割り込みフラグ有効無効設定を設定する |
Yajirushi | 0:3807ce385b2a | 3104 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3105 | * 引数mask: 各センサーの割り込みフラグ有効無効設定のレジスタ値 |
Yajirushi | 0:3807ce385b2a | 3106 | * 2bit目 角速度センサーAnyMotion割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3107 | * 3bit目 角速度センサーHighRate割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3108 | * 5bit目 加速度センサーHIGH_G割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3109 | * 6bit目 加速度センサーAnyMotion割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3110 | * 7bit目 加速度センサーNoMotion(SloMo)割り込み(0:無効, 1:有効) |
Yajirushi | 0:3807ce385b2a | 3111 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3112 | * returns: |
Yajirushi | 0:3807ce385b2a | 3113 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3114 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3115 | */ |
Yajirushi | 0:3807ce385b2a | 3116 | char BOARDC_BNO055::setInterruptEnable(char mask){ |
Yajirushi | 0:3807ce385b2a | 3117 | return ctrl->wr(1, BNO055P1_INT_EN, mask); |
Yajirushi | 0:3807ce385b2a | 3118 | } |
Yajirushi | 0:3807ce385b2a | 3119 | |
Yajirushi | 0:3807ce385b2a | 3120 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3121 | * 加速度センサーのAnyMotion割り込み発生閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3122 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3123 | * 引数ismg: 出力する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3124 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3125 | * returns: |
Yajirushi | 0:3807ce385b2a | 3126 | * 加速度センサーのAnyMotion割り込み発生閾値 |
Yajirushi | 0:3807ce385b2a | 3127 | */ |
Yajirushi | 0:3807ce385b2a | 3128 | float BOARDC_BNO055::getAccAnyMotionThreashold(bool ismg){ |
Yajirushi | 0:3807ce385b2a | 3129 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3130 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3131 | |
Yajirushi | 0:3807ce385b2a | 3132 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3133 | case 0: |
Yajirushi | 0:3807ce385b2a | 3134 | scale = 3.91; |
Yajirushi | 0:3807ce385b2a | 3135 | break; |
Yajirushi | 0:3807ce385b2a | 3136 | case 1: |
Yajirushi | 0:3807ce385b2a | 3137 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3138 | break; |
Yajirushi | 0:3807ce385b2a | 3139 | case 2: |
Yajirushi | 0:3807ce385b2a | 3140 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3141 | break; |
Yajirushi | 0:3807ce385b2a | 3142 | case 3: |
Yajirushi | 0:3807ce385b2a | 3143 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3144 | break; |
Yajirushi | 0:3807ce385b2a | 3145 | } |
Yajirushi | 0:3807ce385b2a | 3146 | |
Yajirushi | 0:3807ce385b2a | 3147 | scale *= (ismg) ? 1.0 : 9.80665; |
Yajirushi | 0:3807ce385b2a | 3148 | |
Yajirushi | 0:3807ce385b2a | 3149 | return (1.0 * ctrl->rr(1, BNO055P1_ACC_AM_THRES)) * scale; |
Yajirushi | 0:3807ce385b2a | 3150 | } |
Yajirushi | 0:3807ce385b2a | 3151 | |
Yajirushi | 0:3807ce385b2a | 3152 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3153 | * 加速度センサーのAnyMotion割り込み発生閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3154 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3155 | * 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3156 | * 引数threashold: 単位変換済みの閾値 |
Yajirushi | 0:3807ce385b2a | 3157 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3158 | * returns: |
Yajirushi | 0:3807ce385b2a | 3159 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3160 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3161 | */ |
Yajirushi | 0:3807ce385b2a | 3162 | char BOARDC_BNO055::setAccAnyMotionThreashold(bool ismg, float threashold){ |
Yajirushi | 0:3807ce385b2a | 3163 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3164 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3165 | |
Yajirushi | 0:3807ce385b2a | 3166 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3167 | case 0: |
Yajirushi | 0:3807ce385b2a | 3168 | scale = 3.91; |
Yajirushi | 0:3807ce385b2a | 3169 | break; |
Yajirushi | 0:3807ce385b2a | 3170 | case 1: |
Yajirushi | 0:3807ce385b2a | 3171 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3172 | break; |
Yajirushi | 0:3807ce385b2a | 3173 | case 2: |
Yajirushi | 0:3807ce385b2a | 3174 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3175 | break; |
Yajirushi | 0:3807ce385b2a | 3176 | case 3: |
Yajirushi | 0:3807ce385b2a | 3177 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3178 | break; |
Yajirushi | 0:3807ce385b2a | 3179 | } |
Yajirushi | 0:3807ce385b2a | 3180 | |
Yajirushi | 0:3807ce385b2a | 3181 | scale *= (ismg) ? 1.0 : 9.8; |
Yajirushi | 0:3807ce385b2a | 3182 | |
Yajirushi | 0:3807ce385b2a | 3183 | char cTh = (char)((threashold / scale) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3184 | |
Yajirushi | 0:3807ce385b2a | 3185 | return ctrl->wr(1, BNO055P1_ACC_AM_THRES, cTh); |
Yajirushi | 0:3807ce385b2a | 3186 | } |
Yajirushi | 0:3807ce385b2a | 3187 | |
Yajirushi | 0:3807ce385b2a | 3188 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3189 | * 加速度センサーの割り込み使用設定を取得する |
Yajirushi | 0:3807ce385b2a | 3190 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3191 | * returns: |
Yajirushi | 0:3807ce385b2a | 3192 | * 0bit目,1bit目 連続発生閾値([1bit目+0bit目+1]回の発生でフラグON) |
Yajirushi | 0:3807ce385b2a | 3193 | * 2bit目 AM/NM_X_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3194 | * 3bit目 AM/NM_Y_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3195 | * 4bit目 AM/NM_Z_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3196 | * 5bit目 HG_X_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3197 | * 6bit目 HG_Y_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3198 | * 7bit目 HG_Z_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3199 | */ |
Yajirushi | 0:3807ce385b2a | 3200 | char BOARDC_BNO055::getAccInterruptSettings(){ |
Yajirushi | 0:3807ce385b2a | 3201 | return ctrl->rr(1, BNO055P1_ACC_INT_SETTINGS); |
Yajirushi | 0:3807ce385b2a | 3202 | } |
Yajirushi | 0:3807ce385b2a | 3203 | |
Yajirushi | 0:3807ce385b2a | 3204 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3205 | * 加速度センサーの割り込み使用設定を設定する |
Yajirushi | 0:3807ce385b2a | 3206 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3207 | * 引数settings: |
Yajirushi | 0:3807ce385b2a | 3208 | * 0bit目,1bit目 連続発生閾値([1bit目+0bit目+1]回の発生でフラグON) |
Yajirushi | 0:3807ce385b2a | 3209 | * 2bit目 AM/NM_X_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3210 | * 3bit目 AM/NM_Y_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3211 | * 4bit目 AM/NM_Z_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3212 | * 5bit目 HG_X_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3213 | * 6bit目 HG_Y_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3214 | * 7bit目 HG_Z_AXISトリガー使用(0:使用しない, 1:使用する) |
Yajirushi | 0:3807ce385b2a | 3215 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3216 | * returns: |
Yajirushi | 0:3807ce385b2a | 3217 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3218 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3219 | */ |
Yajirushi | 0:3807ce385b2a | 3220 | char BOARDC_BNO055::setAccInterruptSettings(char settings){ |
Yajirushi | 0:3807ce385b2a | 3221 | return ctrl->wr(1, BNO055P1_ACC_INT_SETTINGS, settings); |
Yajirushi | 0:3807ce385b2a | 3222 | } |
Yajirushi | 0:3807ce385b2a | 3223 | |
Yajirushi | 0:3807ce385b2a | 3224 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3225 | * 加速度センサーのHighG割り込み閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3226 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3227 | * returns: |
Yajirushi | 0:3807ce385b2a | 3228 | * HighG割り込み発生閾値[単位:ミリ秒](2ms - 512ms) |
Yajirushi | 0:3807ce385b2a | 3229 | */ |
Yajirushi | 0:3807ce385b2a | 3230 | unsigned short BOARDC_BNO055::getAccHighGduration(){ |
Yajirushi | 0:3807ce385b2a | 3231 | return (ctrl->rr(1, BNO055P1_ACC_HG_DURATION) + 1) << 1; |
Yajirushi | 0:3807ce385b2a | 3232 | } |
Yajirushi | 0:3807ce385b2a | 3233 | |
Yajirushi | 0:3807ce385b2a | 3234 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3235 | * 加速度センサーのHighG割り込み継続発生閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3236 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3237 | * 引数ms: HighG割り込み継続発生閾値[単位:ミリ秒](2ms - 512ms) |
Yajirushi | 0:3807ce385b2a | 3238 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3239 | * returns: |
Yajirushi | 0:3807ce385b2a | 3240 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3241 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3242 | */ |
Yajirushi | 0:3807ce385b2a | 3243 | char BOARDC_BNO055::setAccHighGduration(short ms){ |
Yajirushi | 0:3807ce385b2a | 3244 | if(ms > 512 || ms < 2) return -1; |
Yajirushi | 0:3807ce385b2a | 3245 | |
Yajirushi | 0:3807ce385b2a | 3246 | ms = (ms >> 1) - 1; |
Yajirushi | 0:3807ce385b2a | 3247 | |
Yajirushi | 0:3807ce385b2a | 3248 | return ctrl->wr(1, BNO055P1_ACC_HG_DURATION, ms & 0xFF); |
Yajirushi | 0:3807ce385b2a | 3249 | } |
Yajirushi | 0:3807ce385b2a | 3250 | |
Yajirushi | 0:3807ce385b2a | 3251 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3252 | * 加速度センサーのHighG割り込み発生閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3253 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3254 | * 引数ismg: 出力する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3255 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3256 | * returns: |
Yajirushi | 0:3807ce385b2a | 3257 | * HighG割り込み発生閾値 |
Yajirushi | 0:3807ce385b2a | 3258 | */ |
Yajirushi | 0:3807ce385b2a | 3259 | float BOARDC_BNO055::getAccHighGThreashold(bool ismg){ |
Yajirushi | 0:3807ce385b2a | 3260 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3261 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3262 | |
Yajirushi | 0:3807ce385b2a | 3263 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3264 | case 0: |
Yajirushi | 0:3807ce385b2a | 3265 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3266 | break; |
Yajirushi | 0:3807ce385b2a | 3267 | case 1: |
Yajirushi | 0:3807ce385b2a | 3268 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3269 | break; |
Yajirushi | 0:3807ce385b2a | 3270 | case 2: |
Yajirushi | 0:3807ce385b2a | 3271 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3272 | break; |
Yajirushi | 0:3807ce385b2a | 3273 | case 3: |
Yajirushi | 0:3807ce385b2a | 3274 | scale = 62.5; |
Yajirushi | 0:3807ce385b2a | 3275 | break; |
Yajirushi | 0:3807ce385b2a | 3276 | } |
Yajirushi | 0:3807ce385b2a | 3277 | |
Yajirushi | 0:3807ce385b2a | 3278 | scale *= (ismg) ? 1.0 : 9.8; |
Yajirushi | 0:3807ce385b2a | 3279 | |
Yajirushi | 0:3807ce385b2a | 3280 | return (1.0 * ctrl->rr(1, BNO055P1_ACC_HG_THRES)) * scale; |
Yajirushi | 0:3807ce385b2a | 3281 | } |
Yajirushi | 0:3807ce385b2a | 3282 | |
Yajirushi | 0:3807ce385b2a | 3283 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3284 | * 加速度センサーのHighG割り込み発生閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3285 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3286 | * 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3287 | * 引数threashold: 単位変換済みの閾値 |
Yajirushi | 0:3807ce385b2a | 3288 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3289 | * returns: |
Yajirushi | 0:3807ce385b2a | 3290 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3291 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3292 | */ |
Yajirushi | 0:3807ce385b2a | 3293 | char BOARDC_BNO055::setAccHighGThreashold(bool ismg, float threashold){ |
Yajirushi | 0:3807ce385b2a | 3294 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3295 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3296 | |
Yajirushi | 0:3807ce385b2a | 3297 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3298 | case 0: |
Yajirushi | 0:3807ce385b2a | 3299 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3300 | break; |
Yajirushi | 0:3807ce385b2a | 3301 | case 1: |
Yajirushi | 0:3807ce385b2a | 3302 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3303 | break; |
Yajirushi | 0:3807ce385b2a | 3304 | case 2: |
Yajirushi | 0:3807ce385b2a | 3305 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3306 | break; |
Yajirushi | 0:3807ce385b2a | 3307 | case 3: |
Yajirushi | 0:3807ce385b2a | 3308 | scale = 62.5; |
Yajirushi | 0:3807ce385b2a | 3309 | break; |
Yajirushi | 0:3807ce385b2a | 3310 | } |
Yajirushi | 0:3807ce385b2a | 3311 | |
Yajirushi | 0:3807ce385b2a | 3312 | scale *= (ismg) ? 1.0 : 9.8; |
Yajirushi | 0:3807ce385b2a | 3313 | |
Yajirushi | 0:3807ce385b2a | 3314 | char cTh = (char)((threashold / scale) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3315 | |
Yajirushi | 0:3807ce385b2a | 3316 | return ctrl->wr(1, BNO055P1_ACC_HG_THRES, cTh); |
Yajirushi | 0:3807ce385b2a | 3317 | } |
Yajirushi | 0:3807ce385b2a | 3318 | |
Yajirushi | 0:3807ce385b2a | 3319 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3320 | * 加速度センサーのNoMotion割り込み発生閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3321 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3322 | * 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3323 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3324 | * returns: |
Yajirushi | 0:3807ce385b2a | 3325 | * NoMotion割り込み発生閾値 |
Yajirushi | 0:3807ce385b2a | 3326 | */ |
Yajirushi | 0:3807ce385b2a | 3327 | float BOARDC_BNO055::getAccNMThreashold(bool ismg){ |
Yajirushi | 0:3807ce385b2a | 3328 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3329 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3330 | |
Yajirushi | 0:3807ce385b2a | 3331 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3332 | case 0: |
Yajirushi | 0:3807ce385b2a | 3333 | scale = 3.91; |
Yajirushi | 0:3807ce385b2a | 3334 | break; |
Yajirushi | 0:3807ce385b2a | 3335 | case 1: |
Yajirushi | 0:3807ce385b2a | 3336 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3337 | break; |
Yajirushi | 0:3807ce385b2a | 3338 | case 2: |
Yajirushi | 0:3807ce385b2a | 3339 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3340 | break; |
Yajirushi | 0:3807ce385b2a | 3341 | case 3: |
Yajirushi | 0:3807ce385b2a | 3342 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3343 | break; |
Yajirushi | 0:3807ce385b2a | 3344 | } |
Yajirushi | 0:3807ce385b2a | 3345 | |
Yajirushi | 0:3807ce385b2a | 3346 | scale *= (ismg) ? 1.0 : 9.8; |
Yajirushi | 0:3807ce385b2a | 3347 | |
Yajirushi | 0:3807ce385b2a | 3348 | return (1.0 * ctrl->rr(1, BNO055P1_ACC_NM_THRES)) * scale; |
Yajirushi | 0:3807ce385b2a | 3349 | } |
Yajirushi | 0:3807ce385b2a | 3350 | |
Yajirushi | 0:3807ce385b2a | 3351 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3352 | * 加速度センサーのNoMotion割り込み発生閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3353 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3354 | * 引数ismg: 設定する値の単位の選択(true: mG, false: mm/s^2) |
Yajirushi | 0:3807ce385b2a | 3355 | * 引数threashold: 単位変換済みの閾値 |
Yajirushi | 0:3807ce385b2a | 3356 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3357 | * returns: |
Yajirushi | 0:3807ce385b2a | 3358 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3359 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3360 | */ |
Yajirushi | 0:3807ce385b2a | 3361 | char BOARDC_BNO055::setAccNMThreashold(bool ismg, float threashold){ |
Yajirushi | 0:3807ce385b2a | 3362 | char rc = getAccConfig(); |
Yajirushi | 0:3807ce385b2a | 3363 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3364 | |
Yajirushi | 0:3807ce385b2a | 3365 | switch(rc & 0x03){ |
Yajirushi | 0:3807ce385b2a | 3366 | case 0: |
Yajirushi | 0:3807ce385b2a | 3367 | scale = 3.91; |
Yajirushi | 0:3807ce385b2a | 3368 | break; |
Yajirushi | 0:3807ce385b2a | 3369 | case 1: |
Yajirushi | 0:3807ce385b2a | 3370 | scale = 7.81; |
Yajirushi | 0:3807ce385b2a | 3371 | break; |
Yajirushi | 0:3807ce385b2a | 3372 | case 2: |
Yajirushi | 0:3807ce385b2a | 3373 | scale = 15.63; |
Yajirushi | 0:3807ce385b2a | 3374 | break; |
Yajirushi | 0:3807ce385b2a | 3375 | case 3: |
Yajirushi | 0:3807ce385b2a | 3376 | scale = 31.25; |
Yajirushi | 0:3807ce385b2a | 3377 | break; |
Yajirushi | 0:3807ce385b2a | 3378 | } |
Yajirushi | 0:3807ce385b2a | 3379 | |
Yajirushi | 0:3807ce385b2a | 3380 | scale *= (ismg) ? 1.0 : 9.8; |
Yajirushi | 0:3807ce385b2a | 3381 | |
Yajirushi | 0:3807ce385b2a | 3382 | char cTh = (char)((threashold / scale) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3383 | |
Yajirushi | 0:3807ce385b2a | 3384 | return ctrl->wr(1, BNO055P1_ACC_NM_THRES, cTh); |
Yajirushi | 0:3807ce385b2a | 3385 | } |
Yajirushi | 0:3807ce385b2a | 3386 | |
Yajirushi | 0:3807ce385b2a | 3387 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3388 | * 加速度センサーのNoMotion割り込み設定を取得する |
Yajirushi | 0:3807ce385b2a | 3389 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3390 | * returns: |
Yajirushi | 0:3807ce385b2a | 3391 | * NoMotion割り込み設定値 |
Yajirushi | 0:3807ce385b2a | 3392 | * 1bit目: スローモーション、ノーモーション選択(0:NoMotion, 1:SlowMotion) |
Yajirushi | 0:3807ce385b2a | 3393 | * 2 - 7bit目: slo_no_mot _dur設定値(データシート参照) |
Yajirushi | 0:3807ce385b2a | 3394 | */ |
Yajirushi | 0:3807ce385b2a | 3395 | char BOARDC_BNO055::getAccNMsetting(){ |
Yajirushi | 0:3807ce385b2a | 3396 | return ctrl->rr(1, BNO055P1_ACC_NM_SET); |
Yajirushi | 0:3807ce385b2a | 3397 | } |
Yajirushi | 0:3807ce385b2a | 3398 | |
Yajirushi | 0:3807ce385b2a | 3399 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3400 | * 加速度センサーのNoMotion割り込み設定を設定する |
Yajirushi | 0:3807ce385b2a | 3401 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3402 | * 引数setting: NoMotion割り込み設定値 |
Yajirushi | 0:3807ce385b2a | 3403 | * 1bit目: スローモーション、ノーモーション選択(0:NoMotion, 1:SlowMotion) |
Yajirushi | 0:3807ce385b2a | 3404 | * 2 - 7bit目: slo_no_mot _dur設定値(データシート参照) |
Yajirushi | 0:3807ce385b2a | 3405 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3406 | * returns: |
Yajirushi | 0:3807ce385b2a | 3407 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3408 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3409 | */ |
Yajirushi | 0:3807ce385b2a | 3410 | char BOARDC_BNO055::setAccNMsetting(char setting){ |
Yajirushi | 0:3807ce385b2a | 3411 | return ctrl->wr(1, BNO055P1_ACC_NM_SET, setting); |
Yajirushi | 0:3807ce385b2a | 3412 | } |
Yajirushi | 0:3807ce385b2a | 3413 | |
Yajirushi | 0:3807ce385b2a | 3414 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3415 | * 角速度センサーの割り込み設定を取得する |
Yajirushi | 0:3807ce385b2a | 3416 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3417 | * returns: |
Yajirushi | 0:3807ce385b2a | 3418 | * 角速度センサーの割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3419 | */ |
Yajirushi | 0:3807ce385b2a | 3420 | char BOARDC_BNO055::getGyroInterruptSettings(){ |
Yajirushi | 0:3807ce385b2a | 3421 | return ctrl->rr(1, BNO055P1_GYR_INT_SETING); |
Yajirushi | 0:3807ce385b2a | 3422 | } |
Yajirushi | 0:3807ce385b2a | 3423 | |
Yajirushi | 0:3807ce385b2a | 3424 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3425 | * 角速度センサーの割り込み設定を設定する |
Yajirushi | 0:3807ce385b2a | 3426 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3427 | * 引数setting: 角速度センサーの割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3428 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3429 | * returns: |
Yajirushi | 0:3807ce385b2a | 3430 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3431 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3432 | */ |
Yajirushi | 0:3807ce385b2a | 3433 | char BOARDC_BNO055::setGyroInterruptSettings(char settings){ |
Yajirushi | 0:3807ce385b2a | 3434 | return ctrl->wr(1, BNO055P1_GYR_INT_SETING, settings); |
Yajirushi | 0:3807ce385b2a | 3435 | } |
Yajirushi | 0:3807ce385b2a | 3436 | |
Yajirushi | 0:3807ce385b2a | 3437 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3438 | * 角速度センサーのHighRate割り込み設定(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3439 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3440 | * 引数setting: 角速度センサーの割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3441 | */ |
Yajirushi | 0:3807ce385b2a | 3442 | char BOARDC_BNO055::getGyroHighRateXsetting(){ |
Yajirushi | 0:3807ce385b2a | 3443 | return ctrl->rr(1, BNO055P1_GYR_HR_X_SET); |
Yajirushi | 0:3807ce385b2a | 3444 | } |
Yajirushi | 0:3807ce385b2a | 3445 | |
Yajirushi | 0:3807ce385b2a | 3446 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3447 | * 角速度センサーのHighRate割り込み設定(X軸のみ、実際の値)を取得する |
Yajirushi | 0:3807ce385b2a | 3448 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3449 | * 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される |
Yajirushi | 0:3807ce385b2a | 3450 | * 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される |
Yajirushi | 0:3807ce385b2a | 3451 | */ |
Yajirushi | 0:3807ce385b2a | 3452 | void BOARDC_BNO055::getGyroHighRateXsetting_dps(float &hyst, float &thres){ |
Yajirushi | 0:3807ce385b2a | 3453 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3454 | hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3455 | thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3456 | |
Yajirushi | 0:3807ce385b2a | 3457 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3458 | case 0: |
Yajirushi | 0:3807ce385b2a | 3459 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3460 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3461 | break; |
Yajirushi | 0:3807ce385b2a | 3462 | case 1: |
Yajirushi | 0:3807ce385b2a | 3463 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3464 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3465 | break; |
Yajirushi | 0:3807ce385b2a | 3466 | case 2: |
Yajirushi | 0:3807ce385b2a | 3467 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3468 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3469 | break; |
Yajirushi | 0:3807ce385b2a | 3470 | case 3: |
Yajirushi | 0:3807ce385b2a | 3471 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3472 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3473 | break; |
Yajirushi | 0:3807ce385b2a | 3474 | case 4: |
Yajirushi | 0:3807ce385b2a | 3475 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3476 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3477 | break; |
Yajirushi | 0:3807ce385b2a | 3478 | } |
Yajirushi | 0:3807ce385b2a | 3479 | |
Yajirushi | 0:3807ce385b2a | 3480 | char val = ctrl->rr(1, BNO055P1_GYR_HR_X_SET); |
Yajirushi | 0:3807ce385b2a | 3481 | |
Yajirushi | 0:3807ce385b2a | 3482 | hyst *= (float)((val & 0x60) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3483 | thres *= (float)((val & 0x1F) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3484 | } |
Yajirushi | 0:3807ce385b2a | 3485 | |
Yajirushi | 0:3807ce385b2a | 3486 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3487 | * 角速度センサーのHighRate割り込み設定(X軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3488 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3489 | * 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3490 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3491 | * returns: |
Yajirushi | 0:3807ce385b2a | 3492 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3493 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3494 | */ |
Yajirushi | 0:3807ce385b2a | 3495 | char BOARDC_BNO055::setGyroHighRateXsetting(char setting){ |
Yajirushi | 0:3807ce385b2a | 3496 | return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, setting); |
Yajirushi | 0:3807ce385b2a | 3497 | } |
Yajirushi | 0:3807ce385b2a | 3498 | |
Yajirushi | 0:3807ce385b2a | 3499 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3500 | * 角速度センサーのHighRate割り込み設定(X軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3501 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3502 | * 引数hystVal: ヒステリシス |
Yajirushi | 0:3807ce385b2a | 3503 | * 引数thresVal: スレッショルド |
Yajirushi | 0:3807ce385b2a | 3504 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3505 | * returns: |
Yajirushi | 0:3807ce385b2a | 3506 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3507 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3508 | */ |
Yajirushi | 0:3807ce385b2a | 3509 | char BOARDC_BNO055::setGyroHighRateXsetting_dps(float hystVal, float thresVal){ |
Yajirushi | 0:3807ce385b2a | 3510 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3511 | float hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3512 | float thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3513 | |
Yajirushi | 0:3807ce385b2a | 3514 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3515 | case 0: |
Yajirushi | 0:3807ce385b2a | 3516 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3517 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3518 | break; |
Yajirushi | 0:3807ce385b2a | 3519 | case 1: |
Yajirushi | 0:3807ce385b2a | 3520 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3521 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3522 | break; |
Yajirushi | 0:3807ce385b2a | 3523 | case 2: |
Yajirushi | 0:3807ce385b2a | 3524 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3525 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3526 | break; |
Yajirushi | 0:3807ce385b2a | 3527 | case 3: |
Yajirushi | 0:3807ce385b2a | 3528 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3529 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3530 | break; |
Yajirushi | 0:3807ce385b2a | 3531 | case 4: |
Yajirushi | 0:3807ce385b2a | 3532 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3533 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3534 | break; |
Yajirushi | 0:3807ce385b2a | 3535 | } |
Yajirushi | 0:3807ce385b2a | 3536 | |
Yajirushi | 0:3807ce385b2a | 3537 | char hystChar = (char)((hystVal / hyst) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3538 | char thresChar = (char)((thresVal / thres) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3539 | |
Yajirushi | 0:3807ce385b2a | 3540 | return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, ((hystChar << 5) | thresChar)); |
Yajirushi | 0:3807ce385b2a | 3541 | } |
Yajirushi | 0:3807ce385b2a | 3542 | |
Yajirushi | 0:3807ce385b2a | 3543 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3544 | * 角速度センサーのHighRate割り込み継続発生閾値(X軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3545 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3546 | * returns: |
Yajirushi | 0:3807ce385b2a | 3547 | * HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3548 | */ |
Yajirushi | 0:3807ce385b2a | 3549 | float BOARDC_BNO055::getGyroHighRateXduration(){ |
Yajirushi | 0:3807ce385b2a | 3550 | return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_X)) * 2.5; |
Yajirushi | 0:3807ce385b2a | 3551 | } |
Yajirushi | 0:3807ce385b2a | 3552 | |
Yajirushi | 0:3807ce385b2a | 3553 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3554 | * 角速度センサーのHighRate割り込み継続発生閾値(X軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3555 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3556 | * 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3557 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3558 | * returns: |
Yajirushi | 0:3807ce385b2a | 3559 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3560 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3561 | */ |
Yajirushi | 0:3807ce385b2a | 3562 | char BOARDC_BNO055::setGyroHighRateXduration(float duration){ |
Yajirushi | 0:3807ce385b2a | 3563 | return ctrl->wr(1, BNO055P1_GYR_HR_X_SET, (char)(((duration / 2.5) - 1.0) + 0.5)); |
Yajirushi | 0:3807ce385b2a | 3564 | } |
Yajirushi | 0:3807ce385b2a | 3565 | |
Yajirushi | 0:3807ce385b2a | 3566 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3567 | * 角速度センサーのHighRate割り込み設定(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3568 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3569 | * returns: |
Yajirushi | 0:3807ce385b2a | 3570 | * 角速度センサーのHighRate割り込み設定 |
Yajirushi | 0:3807ce385b2a | 3571 | */ |
Yajirushi | 0:3807ce385b2a | 3572 | char BOARDC_BNO055::getGyroHighRateYsetting(){ |
Yajirushi | 0:3807ce385b2a | 3573 | return ctrl->rr(1, BNO055P1_GYR_HR_Y_SET); |
Yajirushi | 0:3807ce385b2a | 3574 | } |
Yajirushi | 0:3807ce385b2a | 3575 | |
Yajirushi | 0:3807ce385b2a | 3576 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3577 | * 角速度センサーのHighRate割り込み設定(Y軸のみ、実際の値)を取得する |
Yajirushi | 0:3807ce385b2a | 3578 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3579 | * 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される |
Yajirushi | 0:3807ce385b2a | 3580 | * 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される |
Yajirushi | 0:3807ce385b2a | 3581 | */ |
Yajirushi | 0:3807ce385b2a | 3582 | void BOARDC_BNO055::getGyroHighRateYsetting_dps(float &hyst, float &thres){ |
Yajirushi | 0:3807ce385b2a | 3583 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3584 | hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3585 | thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3586 | |
Yajirushi | 0:3807ce385b2a | 3587 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3588 | case 0: |
Yajirushi | 0:3807ce385b2a | 3589 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3590 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3591 | break; |
Yajirushi | 0:3807ce385b2a | 3592 | case 1: |
Yajirushi | 0:3807ce385b2a | 3593 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3594 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3595 | break; |
Yajirushi | 0:3807ce385b2a | 3596 | case 2: |
Yajirushi | 0:3807ce385b2a | 3597 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3598 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3599 | break; |
Yajirushi | 0:3807ce385b2a | 3600 | case 3: |
Yajirushi | 0:3807ce385b2a | 3601 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3602 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3603 | break; |
Yajirushi | 0:3807ce385b2a | 3604 | case 4: |
Yajirushi | 0:3807ce385b2a | 3605 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3606 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3607 | break; |
Yajirushi | 0:3807ce385b2a | 3608 | } |
Yajirushi | 0:3807ce385b2a | 3609 | |
Yajirushi | 0:3807ce385b2a | 3610 | char val = ctrl->rr(1, BNO055P1_GYR_HR_Y_SET); |
Yajirushi | 0:3807ce385b2a | 3611 | |
Yajirushi | 0:3807ce385b2a | 3612 | hyst *= (float)((val & 0x60) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3613 | thres *= (float)((val & 0x1F) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3614 | } |
Yajirushi | 0:3807ce385b2a | 3615 | |
Yajirushi | 0:3807ce385b2a | 3616 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3617 | * 角速度センサーのHighRate割り込み設定(Y軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3618 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3619 | * 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3620 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3621 | * returns: |
Yajirushi | 0:3807ce385b2a | 3622 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3623 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3624 | */ |
Yajirushi | 0:3807ce385b2a | 3625 | char BOARDC_BNO055::setGyroHighRateYsetting(char setting){ |
Yajirushi | 0:3807ce385b2a | 3626 | return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, setting); |
Yajirushi | 0:3807ce385b2a | 3627 | } |
Yajirushi | 0:3807ce385b2a | 3628 | |
Yajirushi | 0:3807ce385b2a | 3629 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3630 | * 角速度センサーのHighRate割り込み設定(Y軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3631 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3632 | * 引数hystVal: ヒステリシス |
Yajirushi | 0:3807ce385b2a | 3633 | * 引数thresVal: スレッショルド |
Yajirushi | 0:3807ce385b2a | 3634 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3635 | * returns: |
Yajirushi | 0:3807ce385b2a | 3636 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3637 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3638 | */ |
Yajirushi | 0:3807ce385b2a | 3639 | char BOARDC_BNO055::setGyroHighRateYsetting_dps(float hystVal, float thresVal){ |
Yajirushi | 0:3807ce385b2a | 3640 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3641 | float hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3642 | float thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3643 | |
Yajirushi | 0:3807ce385b2a | 3644 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3645 | case 0: |
Yajirushi | 0:3807ce385b2a | 3646 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3647 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3648 | break; |
Yajirushi | 0:3807ce385b2a | 3649 | case 1: |
Yajirushi | 0:3807ce385b2a | 3650 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3651 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3652 | break; |
Yajirushi | 0:3807ce385b2a | 3653 | case 2: |
Yajirushi | 0:3807ce385b2a | 3654 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3655 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3656 | break; |
Yajirushi | 0:3807ce385b2a | 3657 | case 3: |
Yajirushi | 0:3807ce385b2a | 3658 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3659 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3660 | break; |
Yajirushi | 0:3807ce385b2a | 3661 | case 4: |
Yajirushi | 0:3807ce385b2a | 3662 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3663 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3664 | break; |
Yajirushi | 0:3807ce385b2a | 3665 | } |
Yajirushi | 0:3807ce385b2a | 3666 | |
Yajirushi | 0:3807ce385b2a | 3667 | char hystChar = (char)((hystVal / hyst) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3668 | char thresChar = (char)((thresVal / thres) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3669 | |
Yajirushi | 0:3807ce385b2a | 3670 | return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, ((hystChar << 5) | thresChar)); |
Yajirushi | 0:3807ce385b2a | 3671 | } |
Yajirushi | 0:3807ce385b2a | 3672 | |
Yajirushi | 0:3807ce385b2a | 3673 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3674 | * 角速度センサーのHighRate割り込み継続発生閾値(Y軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3675 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3676 | * returns: |
Yajirushi | 0:3807ce385b2a | 3677 | * HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3678 | */ |
Yajirushi | 0:3807ce385b2a | 3679 | float BOARDC_BNO055::getGyroHighRateYduration(){ |
Yajirushi | 0:3807ce385b2a | 3680 | return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_Y)) * 2.5; |
Yajirushi | 0:3807ce385b2a | 3681 | } |
Yajirushi | 0:3807ce385b2a | 3682 | |
Yajirushi | 0:3807ce385b2a | 3683 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3684 | * 角速度センサーのHighRate割り込み継続発生閾値(Y軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3685 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3686 | * 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3687 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3688 | * returns: |
Yajirushi | 0:3807ce385b2a | 3689 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3690 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3691 | */ |
Yajirushi | 0:3807ce385b2a | 3692 | char BOARDC_BNO055::setGyroHighRateYduration(float duration){ |
Yajirushi | 0:3807ce385b2a | 3693 | return ctrl->wr(1, BNO055P1_GYR_HR_Y_SET, (char)(((duration / 2.5) - 1.0) + 0.5)); |
Yajirushi | 0:3807ce385b2a | 3694 | } |
Yajirushi | 0:3807ce385b2a | 3695 | |
Yajirushi | 0:3807ce385b2a | 3696 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3697 | * 角速度センサーのHighRate割り込み設定(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3698 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3699 | * returns: |
Yajirushi | 0:3807ce385b2a | 3700 | * 角速度センサーのHighRate割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3701 | */ |
Yajirushi | 0:3807ce385b2a | 3702 | char BOARDC_BNO055::getGyroHighRateZsetting(){ |
Yajirushi | 0:3807ce385b2a | 3703 | return ctrl->rr(1, BNO055P1_GYR_HR_Z_SET); |
Yajirushi | 0:3807ce385b2a | 3704 | } |
Yajirushi | 0:3807ce385b2a | 3705 | |
Yajirushi | 0:3807ce385b2a | 3706 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3707 | * 角速度センサーのHighRate割り込み設定(Z軸のみ、実際の値)を取得する |
Yajirushi | 0:3807ce385b2a | 3708 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3709 | * 引数&hyst: アドレス参照引数:関数実行後、この変数にヒステリシスの値が格納される |
Yajirushi | 0:3807ce385b2a | 3710 | * 引数&thres: アドレス参照引数:関数実行後、この変数にスレッショルドの値が格納される |
Yajirushi | 0:3807ce385b2a | 3711 | */ |
Yajirushi | 0:3807ce385b2a | 3712 | void BOARDC_BNO055::getGyroHighRateZsetting_dps(float &hyst, float &thres){ |
Yajirushi | 0:3807ce385b2a | 3713 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3714 | hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3715 | thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3716 | |
Yajirushi | 0:3807ce385b2a | 3717 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3718 | case 0: |
Yajirushi | 0:3807ce385b2a | 3719 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3720 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3721 | break; |
Yajirushi | 0:3807ce385b2a | 3722 | case 1: |
Yajirushi | 0:3807ce385b2a | 3723 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3724 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3725 | break; |
Yajirushi | 0:3807ce385b2a | 3726 | case 2: |
Yajirushi | 0:3807ce385b2a | 3727 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3728 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3729 | break; |
Yajirushi | 0:3807ce385b2a | 3730 | case 3: |
Yajirushi | 0:3807ce385b2a | 3731 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3732 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3733 | break; |
Yajirushi | 0:3807ce385b2a | 3734 | case 4: |
Yajirushi | 0:3807ce385b2a | 3735 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3736 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3737 | break; |
Yajirushi | 0:3807ce385b2a | 3738 | } |
Yajirushi | 0:3807ce385b2a | 3739 | |
Yajirushi | 0:3807ce385b2a | 3740 | char val = ctrl->rr(1, BNO055P1_GYR_HR_Z_SET); |
Yajirushi | 0:3807ce385b2a | 3741 | |
Yajirushi | 0:3807ce385b2a | 3742 | hyst *= (float)((val & 0x60) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3743 | thres *= (float)((val & 0x1F) * 1.0); |
Yajirushi | 0:3807ce385b2a | 3744 | } |
Yajirushi | 0:3807ce385b2a | 3745 | |
Yajirushi | 0:3807ce385b2a | 3746 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3747 | * 角速度センサーのHighRate割り込み設定(Z軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3748 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3749 | * 引数setting: 角速度センサーのHighRate割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3750 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3751 | * returns: |
Yajirushi | 0:3807ce385b2a | 3752 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3753 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3754 | */ |
Yajirushi | 0:3807ce385b2a | 3755 | char BOARDC_BNO055::setGyroHighRateZsetting(char setting){ |
Yajirushi | 0:3807ce385b2a | 3756 | return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, setting); |
Yajirushi | 0:3807ce385b2a | 3757 | } |
Yajirushi | 0:3807ce385b2a | 3758 | |
Yajirushi | 0:3807ce385b2a | 3759 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3760 | * 角速度センサーのHighRate割り込み設定(Z軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3761 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3762 | * 引数hystVal: ヒステリシス |
Yajirushi | 0:3807ce385b2a | 3763 | * 引数thresVal: スレッショルド |
Yajirushi | 0:3807ce385b2a | 3764 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3765 | * returns: |
Yajirushi | 0:3807ce385b2a | 3766 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3767 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3768 | */ |
Yajirushi | 0:3807ce385b2a | 3769 | char BOARDC_BNO055::setGyroHighRateZsetting_dps(float hystVal, float thresVal){ |
Yajirushi | 0:3807ce385b2a | 3770 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3771 | float hyst = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3772 | float thres = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3773 | |
Yajirushi | 0:3807ce385b2a | 3774 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3775 | case 0: |
Yajirushi | 0:3807ce385b2a | 3776 | hyst = 62.26; |
Yajirushi | 0:3807ce385b2a | 3777 | thres = 62.5; |
Yajirushi | 0:3807ce385b2a | 3778 | break; |
Yajirushi | 0:3807ce385b2a | 3779 | case 1: |
Yajirushi | 0:3807ce385b2a | 3780 | hyst = 31.13; |
Yajirushi | 0:3807ce385b2a | 3781 | thres = 31.25; |
Yajirushi | 0:3807ce385b2a | 3782 | break; |
Yajirushi | 0:3807ce385b2a | 3783 | case 2: |
Yajirushi | 0:3807ce385b2a | 3784 | hyst = 15.56; |
Yajirushi | 0:3807ce385b2a | 3785 | thres = 15.62; |
Yajirushi | 0:3807ce385b2a | 3786 | break; |
Yajirushi | 0:3807ce385b2a | 3787 | case 3: |
Yajirushi | 0:3807ce385b2a | 3788 | hyst = 7.78; |
Yajirushi | 0:3807ce385b2a | 3789 | thres = 7.81; |
Yajirushi | 0:3807ce385b2a | 3790 | break; |
Yajirushi | 0:3807ce385b2a | 3791 | case 4: |
Yajirushi | 0:3807ce385b2a | 3792 | hyst = 3.89; |
Yajirushi | 0:3807ce385b2a | 3793 | thres = 3.90; |
Yajirushi | 0:3807ce385b2a | 3794 | break; |
Yajirushi | 0:3807ce385b2a | 3795 | } |
Yajirushi | 0:3807ce385b2a | 3796 | |
Yajirushi | 0:3807ce385b2a | 3797 | char hystChar = (char)((hystVal / hyst) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3798 | char thresChar = (char)((thresVal / thres) + 0.5); |
Yajirushi | 0:3807ce385b2a | 3799 | |
Yajirushi | 0:3807ce385b2a | 3800 | return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, ((hystChar << 5) | thresChar)); |
Yajirushi | 0:3807ce385b2a | 3801 | } |
Yajirushi | 0:3807ce385b2a | 3802 | |
Yajirushi | 0:3807ce385b2a | 3803 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3804 | * 角速度センサーのHighRate割り込み継続発生閾値(Z軸のみ)を取得する |
Yajirushi | 0:3807ce385b2a | 3805 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3806 | * returns: |
Yajirushi | 0:3807ce385b2a | 3807 | * HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3808 | */ |
Yajirushi | 0:3807ce385b2a | 3809 | float BOARDC_BNO055::getGyroHighRateZduration(){ |
Yajirushi | 0:3807ce385b2a | 3810 | return (float)(1 + ctrl->rr(1, BNO055P1_GYR_DUR_Z)) * 2.5; |
Yajirushi | 0:3807ce385b2a | 3811 | } |
Yajirushi | 0:3807ce385b2a | 3812 | |
Yajirushi | 0:3807ce385b2a | 3813 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3814 | * 角速度センサーのHighRate割り込み継続発生閾値(Z軸のみ)を設定する |
Yajirushi | 0:3807ce385b2a | 3815 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3816 | * 引数duration: HighRate割り込み継続発生閾値[単位:ミリ秒] |
Yajirushi | 0:3807ce385b2a | 3817 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3818 | * returns: |
Yajirushi | 0:3807ce385b2a | 3819 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3820 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3821 | */ |
Yajirushi | 0:3807ce385b2a | 3822 | char BOARDC_BNO055::setGyroHighRateZduration(float duration){ |
Yajirushi | 0:3807ce385b2a | 3823 | return ctrl->wr(1, BNO055P1_GYR_HR_Z_SET, (char)(((duration / 2.5) - 1.0) + 0.5)); |
Yajirushi | 0:3807ce385b2a | 3824 | } |
Yajirushi | 0:3807ce385b2a | 3825 | |
Yajirushi | 0:3807ce385b2a | 3826 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3827 | * 角速度センサーのAnyMotion割り込み閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3828 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3829 | * returns: |
Yajirushi | 0:3807ce385b2a | 3830 | * AnyMotion割り込み閾値[単位:dps] |
Yajirushi | 0:3807ce385b2a | 3831 | */ |
Yajirushi | 0:3807ce385b2a | 3832 | float BOARDC_BNO055::getGyroAnyMotionThreashold(){ |
Yajirushi | 0:3807ce385b2a | 3833 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3834 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3835 | |
Yajirushi | 0:3807ce385b2a | 3836 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3837 | case 0: |
Yajirushi | 0:3807ce385b2a | 3838 | scale = 1.0f; |
Yajirushi | 0:3807ce385b2a | 3839 | break; |
Yajirushi | 0:3807ce385b2a | 3840 | case 1: |
Yajirushi | 0:3807ce385b2a | 3841 | scale = 0.5; |
Yajirushi | 0:3807ce385b2a | 3842 | break; |
Yajirushi | 0:3807ce385b2a | 3843 | case 2: |
Yajirushi | 0:3807ce385b2a | 3844 | scale = 0.25; |
Yajirushi | 0:3807ce385b2a | 3845 | break; |
Yajirushi | 0:3807ce385b2a | 3846 | case 3: |
Yajirushi | 0:3807ce385b2a | 3847 | scale = 0.125; |
Yajirushi | 0:3807ce385b2a | 3848 | break; |
Yajirushi | 0:3807ce385b2a | 3849 | case 4: |
Yajirushi | 0:3807ce385b2a | 3850 | scale = 0.0625; |
Yajirushi | 0:3807ce385b2a | 3851 | break; |
Yajirushi | 0:3807ce385b2a | 3852 | } |
Yajirushi | 0:3807ce385b2a | 3853 | |
Yajirushi | 0:3807ce385b2a | 3854 | return (1.0f * ctrl->rr(1, BNO055P1_GYR_AM_THRES)) * scale; |
Yajirushi | 0:3807ce385b2a | 3855 | } |
Yajirushi | 0:3807ce385b2a | 3856 | |
Yajirushi | 0:3807ce385b2a | 3857 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3858 | * 角速度センサーのAnyMotion割り込み閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3859 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3860 | * 引数threashold: AnyMotion割り込み閾値[単位:dps] |
Yajirushi | 0:3807ce385b2a | 3861 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3862 | * returns: |
Yajirushi | 0:3807ce385b2a | 3863 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3864 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3865 | */ |
Yajirushi | 0:3807ce385b2a | 3866 | char BOARDC_BNO055::setGyroAnyMotionThreashold(float threashold){ |
Yajirushi | 0:3807ce385b2a | 3867 | char r = getGyroConfig_0(); |
Yajirushi | 0:3807ce385b2a | 3868 | float scale = 0.0f; |
Yajirushi | 0:3807ce385b2a | 3869 | |
Yajirushi | 0:3807ce385b2a | 3870 | switch(r & 0x07){ |
Yajirushi | 0:3807ce385b2a | 3871 | case 0: |
Yajirushi | 0:3807ce385b2a | 3872 | scale = 1.0f; |
Yajirushi | 0:3807ce385b2a | 3873 | break; |
Yajirushi | 0:3807ce385b2a | 3874 | case 1: |
Yajirushi | 0:3807ce385b2a | 3875 | scale = 0.5; |
Yajirushi | 0:3807ce385b2a | 3876 | break; |
Yajirushi | 0:3807ce385b2a | 3877 | case 2: |
Yajirushi | 0:3807ce385b2a | 3878 | scale = 0.25; |
Yajirushi | 0:3807ce385b2a | 3879 | break; |
Yajirushi | 0:3807ce385b2a | 3880 | case 3: |
Yajirushi | 0:3807ce385b2a | 3881 | scale = 0.125; |
Yajirushi | 0:3807ce385b2a | 3882 | break; |
Yajirushi | 0:3807ce385b2a | 3883 | case 4: |
Yajirushi | 0:3807ce385b2a | 3884 | scale = 0.0625; |
Yajirushi | 0:3807ce385b2a | 3885 | break; |
Yajirushi | 0:3807ce385b2a | 3886 | } |
Yajirushi | 0:3807ce385b2a | 3887 | |
Yajirushi | 0:3807ce385b2a | 3888 | return ctrl->wr(1, BNO055P1_GYR_AM_THRES, (char)((threashold / scale) + 0.5)); |
Yajirushi | 0:3807ce385b2a | 3889 | } |
Yajirushi | 0:3807ce385b2a | 3890 | |
Yajirushi | 0:3807ce385b2a | 3891 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3892 | * 加速度センサーのAnyMotion割り込み閾値を取得する |
Yajirushi | 0:3807ce385b2a | 3893 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3894 | * returns: |
Yajirushi | 0:3807ce385b2a | 3895 | * AnyMotion割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3896 | */ |
Yajirushi | 0:3807ce385b2a | 3897 | char BOARDC_BNO055::getAccAnyMotionSetting(){ |
Yajirushi | 0:3807ce385b2a | 3898 | return ctrl->rr(1, BNO055P1_GYR_AM_SET); |
Yajirushi | 0:3807ce385b2a | 3899 | } |
Yajirushi | 0:3807ce385b2a | 3900 | |
Yajirushi | 0:3807ce385b2a | 3901 | /* ================================================================== |
Yajirushi | 0:3807ce385b2a | 3902 | * 加速度センサーのAnyMotion割り込み閾値を設定する |
Yajirushi | 0:3807ce385b2a | 3903 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3904 | * 引数setting: AnyMotion割り込み設定レジスタ値 |
Yajirushi | 0:3807ce385b2a | 3905 | * ------------------------------------------------------------------ |
Yajirushi | 0:3807ce385b2a | 3906 | * returns: |
Yajirushi | 0:3807ce385b2a | 3907 | * -1 失敗 |
Yajirushi | 0:3807ce385b2a | 3908 | * 1 成功 |
Yajirushi | 0:3807ce385b2a | 3909 | */ |
Yajirushi | 0:3807ce385b2a | 3910 | char BOARDC_BNO055::setAccAnyMotionSetting(char setting){ |
Yajirushi | 0:3807ce385b2a | 3911 | return ctrl->wr(1, BNO055P1_GYR_AM_SET, setting); |
Yajirushi | 0:3807ce385b2a | 3912 | } |