BNO055.cpp

00001 #include "BNO055.h"
00002 #include "mbed.h"
00003 
00004 DigitalOut AS7000_GPIO8(p5); //NNN50-P5 map to GPIO-8 for AS7000 on/off
00005 DigitalInOut AS7000_INT(p4); //NNN50-P4 GPIO-5 for AS7000 INT
00006 
00007 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
00008     //Set I2C fast and bring reset line high
00009     _i2c.frequency(400000);
00010     address = BNOAddress;
00011     accel_scale = 0.001f;
00012     rate_scale = 1.0f/16.0f;
00013     angle_scale = 1.0f/16.0f;
00014     temp_scale = 1;
00015     
00016     //Set I2C fast and bring reset line high
00017     //_i2c.frequency(400000);
00018     //address = AS7000Address;
00019     
00020     AS7000_INT.mode(PullNone);
00021     
00022     AS7000_INT.output();
00023      
00024     AS7000_GPIO8.write(1); // Power OFF AS7000
00025     AS7000_INT.write(1); // AS7000 GPIO OUTPUT LOW
00026     AS7000_INT.input();
00027     wait_ms(100);    
00028     }
00029     
00030 void BNO055::reset(){
00031 //Perform a power-on-reset
00032     readchar(BNO055_SYS_TRIGGER_ADDR);
00033     rx = rx | 0x20;
00034     writechar(BNO055_SYS_TRIGGER_ADDR,rx);
00035 //Wait for the system to come back up again (datasheet says 650ms)
00036     wait_ms(675);
00037 }
00038     
00039 bool BNO055::check(){
00040 //Check we have communication link with the chip
00041     readchar(BNO055_CHIP_ID_ADDR);
00042     if (rx != 0xA0) return false;
00043 //Grab the chip ID and software versions
00044     tx[0] = BNO055_CHIP_ID_ADDR;
00045     _i2c.write(address,tx,1,true);  
00046     _i2c.read(address+1,rawdata,7,false); 
00047     ID.id = rawdata[0];
00048     ID.accel = rawdata[1];
00049     ID.mag = rawdata[2];
00050     ID.gyro = rawdata[3];
00051     ID.sw[0] = rawdata[4];
00052     ID.sw[1] = rawdata[5];
00053     ID.bootload = rawdata[6];
00054     setpage(1);
00055     tx[0] = BNO055_UNIQUE_ID_ADDR;
00056     _i2c.write(address,tx,1,true);  
00057     _i2c.read(address+1,ID.serial,16,false); 
00058     setpage(0);
00059     return true;
00060     }
00061     
00062 void BNO055::SetExternalCrystal(bool yn){
00063 // Read the current status from the device
00064     readchar(BNO055_SYS_TRIGGER_ADDR); 
00065     if (yn) rx = rx | 0x80;
00066     else rx = rx & 0x7F;
00067     writechar(BNO055_SYS_TRIGGER_ADDR,rx); 
00068 }
00069 
00070 void BNO055::set_accel_units(char units){
00071     readchar(BNO055_UNIT_SEL_ADDR);
00072     if(units == MPERSPERS){
00073         rx = rx & 0xFE;
00074         accel_scale = 0.01f;
00075         }
00076     else {
00077         rx = rx | units;
00078         accel_scale = 0.001f;
00079         }
00080     writechar(BNO055_UNIT_SEL_ADDR,rx);
00081 }
00082 
00083 void BNO055::set_anglerate_units(char units){
00084     readchar(BNO055_UNIT_SEL_ADDR);
00085     if (units == DEG_PER_SEC){
00086         rx = rx & 0xFD;
00087         rate_scale = 1.0f/16.0f;
00088         }
00089     else {
00090         rx = rx | units;
00091         rate_scale = 1.0f/900.0f;
00092         }
00093     writechar(BNO055_UNIT_SEL_ADDR,rx);
00094 }    
00095 
00096 void BNO055::set_angle_units(char units){
00097     readchar(BNO055_UNIT_SEL_ADDR);
00098     if (units == DEGREES){
00099         rx = rx & 0xFB;
00100         angle_scale = 1.0f/16.0f;
00101         }
00102     else {
00103         rx = rx | units;
00104         rate_scale = 1.0f/900.0f;
00105         }
00106     writechar(BNO055_UNIT_SEL_ADDR,rx);
00107 }    
00108 
00109 void BNO055::set_temp_units(char units){
00110     readchar(BNO055_UNIT_SEL_ADDR);
00111     if (units == CENTIGRADE){
00112         rx = rx & 0xEF;
00113         temp_scale = 1;
00114         }
00115     else {
00116         rx = rx | units;
00117         temp_scale = 2;
00118         }
00119     writechar(BNO055_UNIT_SEL_ADDR,rx);
00120 }    
00121 
00122 void BNO055::set_orientation(char units){
00123     readchar(BNO055_UNIT_SEL_ADDR);
00124     if (units == WINDOWS) rx = rx &0x7F;
00125     else rx = rx | units;
00126     writechar(BNO055_UNIT_SEL_ADDR,rx);
00127 }        
00128 
00129 void BNO055::setmode(char omode){
00130     writechar(BNO055_OPR_MODE_ADDR,omode);
00131     op_mode = omode;
00132 }
00133 
00134 void BNO055::setpowermode(char pmode){
00135     writechar(BNO055_PWR_MODE_ADDR,pmode);
00136     pwr_mode = pmode;
00137 }
00138 
00139 void BNO055::get_accel(void){
00140     tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
00141     _i2c.write(address,tx,1,true);  
00142     _i2c.read(address+1,rawdata,6,0); 
00143     accel.rawx = (rawdata[1] << 8 | rawdata[0]);
00144     accel.rawy = (rawdata[3] << 8 | rawdata[2]);
00145     accel.rawz = (rawdata[5] << 8 | rawdata[4]);
00146     accel.x = float(accel.rawx)*accel_scale;
00147     accel.y = float(accel.rawy)*accel_scale;
00148     accel.z = float(accel.rawz)*accel_scale;
00149 }
00150     
00151 void BNO055::get_gyro(void){
00152     tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
00153     _i2c.write(address,tx,1,true);  
00154     _i2c.read(address+1,rawdata,6,0); 
00155     gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
00156     gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
00157     gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
00158     gyro.x = float(gyro.rawx)*rate_scale;
00159     gyro.y = float(gyro.rawy)*rate_scale;
00160     gyro.z = float(gyro.rawz)*rate_scale;
00161 }
00162 
00163 void BNO055::get_mag(void){
00164     tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
00165     _i2c.write(address,tx,1,true);  
00166     _i2c.read(address+1,rawdata,6,0); 
00167     mag.rawx = (rawdata[1] << 8 | rawdata[0]);
00168     mag.rawy = (rawdata[3] << 8 | rawdata[2]);
00169     mag.rawz = (rawdata[5] << 8 | rawdata[4]);
00170     mag.x = float(mag.rawx);
00171     mag.y = float(mag.rawy);
00172     mag.z = float(mag.rawz);
00173 }
00174 
00175 void BNO055::get_lia(void){
00176     tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
00177     _i2c.write(address,tx,1,true);  
00178     _i2c.read(address+1,rawdata,6,0); 
00179     lia.rawx = (rawdata[1] << 8 | rawdata[0]);
00180     lia.rawy = (rawdata[3] << 8 | rawdata[2]);
00181     lia.rawz = (rawdata[5] << 8 | rawdata[4]);
00182     lia.x = float(lia.rawx)*accel_scale;
00183     lia.y = float(lia.rawy)*accel_scale;
00184     lia.z = float(lia.rawz)*accel_scale;
00185 }
00186 
00187 void BNO055::get_grv(void){
00188     tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
00189     _i2c.write(address,tx,1,true);  
00190     _i2c.read(address+1,rawdata,6,0); 
00191     gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
00192     gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
00193     gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
00194     gravity.x = float(gravity.rawx)*accel_scale;
00195     gravity.y = float(gravity.rawy)*accel_scale;
00196     gravity.z = float(gravity.rawz)*accel_scale;
00197 }
00198 
00199 void BNO055::get_quat(void){
00200     tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
00201     _i2c.write(address,tx,1,true);  
00202     _i2c.read(address+1,rawdata,8,0); 
00203     quat.raww = (rawdata[1] << 8 | rawdata[0]);
00204     quat.rawx = (rawdata[3] << 8 | rawdata[2]);
00205     quat.rawy = (rawdata[5] << 8 | rawdata[4]);
00206     quat.rawz = (rawdata[7] << 8 | rawdata[6]);
00207     quat.w = float(quat.raww)/16384.0f;
00208     quat.x = float(quat.rawx)/16384.0f;
00209     quat.y = float(quat.rawy)/16384.0f;
00210     quat.z = float(quat.rawz)/16384.0f;
00211 }
00212 
00213 void BNO055::get_angles(void){
00214     tx[0] = BNO055_EULER_H_LSB_ADDR;
00215     _i2c.write(address,tx,1,true);  
00216     _i2c.read(address+1,rawdata,6,0); 
00217     euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
00218     euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
00219     euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
00220     euler.yaw = float(euler.rawyaw)*angle_scale;
00221     euler.roll = float(euler.rawroll)*angle_scale;
00222     euler.pitch = float(euler.rawpitch)*angle_scale;
00223 }
00224 
00225 
00226 void BNO055::get_temp(void){
00227     readchar(BNO055_TEMP_ADDR);
00228     temperature = rx / temp_scale;
00229 }
00230 
00231 void BNO055::get_calib(void){
00232      readchar(BNO055_CALIB_STAT_ADDR);
00233      calib = rx;
00234 }
00235 
00236 void BNO055::read_calibration_data(void){
00237     char tempmode = op_mode;
00238     setmode(OPERATION_MODE_CONFIG);
00239     wait_ms(20);
00240     tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
00241     _i2c.write(address,tx,1,true);  
00242     _i2c.read(address,calibration,22,false); 
00243     setmode(tempmode);
00244     wait_ms(10);
00245 }
00246 
00247 void BNO055::write_calibration_data(void){
00248     char tempmode = op_mode;
00249     setmode(OPERATION_MODE_CONFIG);
00250     wait_ms(20);
00251     tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
00252     _i2c.write(address,tx,1,true);  
00253     _i2c.write(address,calibration,22,false); 
00254     setmode(tempmode);
00255     wait_ms(10);
00256 }
00257 
00258 void BNO055::set_mapping(char orient){
00259     switch (orient){
00260         case 0: 
00261             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
00262             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
00263             break;
00264         case 1:
00265             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
00266             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
00267             break;
00268         case 2:
00269             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
00270             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
00271             break;
00272         case 3:
00273             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
00274             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
00275             break;
00276         case 4:
00277             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
00278             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
00279             break;
00280         case 5:
00281             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
00282             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
00283             break;
00284         case 6:
00285             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
00286             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
00287             break;
00288         case 7:
00289             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
00290             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
00291             break;
00292         default:
00293             writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
00294             writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
00295         }
00296 }
00297 
00298 //AS7000::AS7000(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
00299 //
00300 //}
00301     
00302 void BNO055::enable(void){
00303         char cmd[2];
00304         
00305         AS7000_GPIO8.write(0); //Power ON AS7000
00306         wait_ms(280);    //Wait for > 250ms for booting
00307           
00308         cmd[0] = AS7000_STARTUP; //0x04;       
00309         cmd[1] = 0x02;
00310         _i2c.write(AS7000Address, cmd, 2);
00311          wait_ms(35);  
00312     
00313 }
00314 
00315 void BNO055::update(void){
00316 
00317 }
00318 
00319 void BNO055::hr_only(void){
00320     char cmd[2];
00321      cmd[0] = HR_ONLY;
00322      if(AS7000_INT == 0) {
00323         _i2c.write(AS7000Address, cmd, 1);
00324         _i2c.read(AS7000Address, rawdata, 7, false);
00325         wait_ms(10);
00326         //update();
00327     hrm.transaction_id = rawdata[0];
00328     hrm.status         = rawdata[1];
00329     hrm.hreat_rate     = rawdata[2];
00330     hrm.signal_QA      = rawdata[3];
00331     hrm.LED_current    = ((rawdata[4]<<8)+rawdata[5]);
00332     hrm.SYNC           = rawdata[6];            
00333     }           
00334 }