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