BMI160.cpp

00001 #include "mbed.h"
00002 #include "BMI160.h"
00003 
00004 /* register address defintions */
00005 #define REG_CHIP_ID       0x00 /* chip id */
00006 #define REG_ERR_REG       0x02 
00007 #define REG_PMU_STATUS    0x03
00008 #define REG_DATA_0        0x04 /* MAG_X_LSB */
00009 #define REG_DATA_1        0x05 /* MAG_X_MSB */
00010 #define REG_DATA_2        0x06 /* MAG_Y_LSB */
00011 #define REG_DATA_3        0x07 /* MAG_Y_MSB */
00012 #define REG_DATA_4        0x08 /* MAG_Z_LSB */
00013 #define REG_DATA_5        0x09 /* MAG_Z_MSB */
00014 #define REG_DATA_6        0x0A /* RHALL_LSB */
00015 #define REG_DATA_7        0x0B /* RHALL_MSB */
00016 #define REG_DATA_8        0x0C /* GRY_X_LSB */
00017 #define REG_DATA_9        0x0D /* GRY_X_MSB */
00018 #define REG_DATA_10       0x0E /* GRY_Y_LSB */
00019 #define REG_DATA_11       0x0F /* GRY_Y_MSB */
00020 #define REG_DATA_12       0x10 /* GRY_Z_LSB */
00021 #define REG_DATA_13       0x11 /* GRY_Z_MSB */
00022 #define REG_DATA_14       0x12 /* ACC_X_LSB */
00023 #define REG_DATA_15       0x13 /* ACC_X_MSB */
00024 #define REG_DATA_16       0x14 /* ACC_Y_LSB */
00025 #define REG_DATA_17       0x15 /* ACC_Y_MSB */
00026 #define REG_DATA_18       0x16 /* ACC_Z_LSB */
00027 #define REG_DATA_19       0x17 /* ACC_Z_MSB */
00028 #define REG_SENSORTIME_0  0x18 /* SENSOR_TIME_LSB */
00029 #define REG_SENSORTIME_1  0x19 /* SENSOR_TIME_CSB */
00030 #define REG_SENSORTIME_2  0x1A /* SENSOR_TIME_MSB */
00031 #define REG_STATUS        0x1B
00032 #define REG_INT_STATUS_0  0x1C
00033 #define REG_INT_STATUS_1  0x1D
00034 #define REG_INT_STATUS_2  0x1E
00035 #define REG_INT_STATUS_3  0x1F
00036 #define REG_TEMPERATURE_0 0x20
00037 #define REG_TEMPERATURE_1 0x21
00038 #define REG_FIFO_LENGTH_0 0x22
00039 #define REG_FIFO_LENGTH_1 0x23
00040 #define REG_FIFO_DATA     0x24
00041 #define REG_ACC_CONF      0x40
00042 #define REG_ACC_RANGE     0x41
00043 #define REG_GYR_CONF      0x42
00044 #define REG_GYR_RANGE     0x43
00045 #define REG_MAG_CONF      0x44
00046 #define REG_FIFO_DOWNS    0x45
00047 #define REG_FIFO_CONFIG_0 0x46
00048 #define REG_FIFO_CONFIG_1 0x47
00049 #define REG_MAG_IF_0      0x4B
00050 #define REG_MAG_IF_1      0x4C
00051 #define REG_MAG_IF_2      0x4D
00052 #define REG_MAG_IF_3      0x4E
00053 #define REG_MAG_IF_4      0x4F
00054 #define REG_INT_EN_0      0x50
00055 #define REG_INT_EN_1      0x51
00056 #define REG_INT_EN_2      0x52
00057 #define REG_INT_OUT_CTRL  0x53
00058 #define REG_INT_LATCH     0x54
00059 #define REG_INT_MAP_0     0x55
00060 #define REG_INT_MAP_1     0x56
00061 #define REG_INT_MAP_2     0x57
00062 #define REG_INT_DATA_0    0x58
00063 #define REG_INT_DATA_1    0x59
00064 #define REG_LOWHIGHT_0    0x5A
00065 #define REG_LOWHIGHT_1    0x5B
00066 #define REG_LOWHIGHT_2    0x5C
00067 #define REG_LOWHIGHT_3    0x5D
00068 #define REG_LOWHIGHT_4    0x5E
00069 #define REG_INT_MOTION_0  0x5F
00070 #define REG_INT_MOTION_1  0x60
00071 #define REG_INT_MOTION_2  0x61
00072 #define REG_INT_MOTION_3  0x62
00073 #define REG_INT_TAP_0     0x63
00074 #define REG_INT_TAP_1     0x64
00075 #define REG_INT_ORIENT_0  0x65
00076 #define REG_INT_ORIENT_1  0x66
00077 #define REG_INT_FLAT_0    0x67
00078 #define REG_INT_FLAT_1    0x68
00079 #define REG_FOC_CONF      0x69
00080 #define REG_CONF          0x6A
00081 #define REG_IF_CONF       0x6B
00082 #define REG_PMU_TRIGGER   0x6C
00083 #define REG_SELF_TEST     0x6D
00084 #define REG_NV_CONF       0x70
00085 #define REG_OFFSET_0      0x71
00086 #define REG_OFFSET_1      0x72
00087 #define REG_OFFSET_2      0x73
00088 #define REG_OFFSET_3      0x74
00089 #define REG_OFFSET_4      0x75
00090 #define REG_OFFSET_5      0x76
00091 #define REG_OFFSET_6      0x77
00092 #define REG_STEP_CNT_0    0x78
00093 #define REG_STEP_CNT_1    0x79
00094 #define REG_STEP_CONF_0   0x7A
00095 #define REG_STEP_CONF_1   0x7B
00096 #define REG_CMD           0x7E
00097 
00098 #define FLOAT_MAX_16BIT   32768.0
00099 
00100 /* 0x00 CHIP_ID reset value = 0xD1 */
00101 /* 0x02 ERR_REG Reports sensor error flags. Flags are cleared when read.
00102  *      bit[7]   mag_drdy_err
00103  *      bit[6]   drop_cmd_err Dropped command to Register
00104  *      bit[5]   i2c_fail_err
00105  *      bit[4:1] err_code error code
00106  *        0000: no error
00107  *        0001: error
00108  *        0010: error
00109  *        0011: low-power mode and interrupt uses pre-filtered data
00110  *        0100-0101: reserved
00111  *        0110: ODRs of enabled sensors in headerless mode do not match
00112  *        0111: pre-filtered data are used in low power mode
00113  *        1000-1111: reserved
00114  *        The first reported error will be shown in the error code
00115  *      bit[0]   fatal_err : Chip not operatable
00116  */
00117 /* 0x03 PMU_STATUS
00118  *      bit[7:6] (reserved)
00119  *      bit[5:4] acc_pmu_status
00120  *      bit[3:2] gyr_pmu_status
00121  *      bit[1:0] mag_pmu_status
00122  */
00123 /* 0x04 DATA_0  : MAG_X[7:0]  */
00124 /* 0x05 DATA_1  : MAG_X[15:8] */
00125 /* 0x06 DATA_2  : MAG_Y[7:0]  */
00126 /* 0x07 DATA_3  : MAG_Y[15:8] */
00127 /* 0x08 DATA_4  : MAG_Z[7:0]  */
00128 /* 0x09 DATA_5  : MAG_Z[15:8] */
00129 /* 0x0A DATA_6  : RHALL[7:0]  */
00130 /* 0x0B DATA_7  : RHALL[15:8] */
00131 /* 0x0C DATA_8  : GYR_X[7:0]  */
00132 /* 0x0D DATA_9  : GYR_X[15:8] */
00133 /* 0x0E DATA_10 : GYR_Y[7:0]  */
00134 /* 0x0F DATA_11 : GYR_Y[15:8] */
00135 /* 0x10 DATA_12 : GYR_Z[7:0]  */
00136 /* 0x11 DATA_13 : GYR_Z[15:8] */
00137 /* 0x12 DATA_14 : ACC_X[7:0]  */
00138 /* 0x13 DATA_15 : ACC_X[15:8] */
00139 /* 0x14 DATA_16 : ACC_Y[7:0]  */
00140 /* 0x15 DATA_17 : ACC_Y[15:8] */
00141 /* 0x16 DATA_18 : ACC_Z[7:0]  */
00142 /* 0x17 DATA_19 : ACC_Z[15:8] */
00143 /* 0x18 SENSORTIME_0 : sensor_time[7:0]   */
00144 /* 0x19 SENSORTIME_1 : sensor_time[15:8]  */
00145 /* 0x1A SENSORTIME_2 : sensor_time[23:16] */
00146 /* 0x1B STATUS
00147  *      bit[7] drdy_acc
00148  *      bit[6] drdy_gyr
00149  *      bit[5] drfy_mag
00150  *      bit[4] nvm_rdy
00151  *      bit[3] foc_rdy
00152  *      bit[2] mag_man_op
00153  *      bit[1] gry_self_test_of
00154  *      bit[0] (reserved)
00155  */
00156 /* 0x1C INT_STATUS_0
00157  *      bit[7] flat_int
00158  *      bit[6] orient_int
00159  *      bit[5] s_tap_int
00160  *      bit[4] d_tap_int
00161  *      bit[3] pmu_trigger_int
00162  *      bit[2] anym_int
00163  *      bit[1] sigmot_int
00164  *      bit[0] step_int
00165  */
00166 /* 0x1D INT_STATUS_1
00167  *      bit[7] nomo_int
00168  *      bit[6] fwm_int
00169  *      bit[5] ffull_int
00170  *      bit[4] drdy_int
00171  *      bit[3] lowg_int
00172  *      bit[2] highg_int
00173  *      bit[1:0] (reserved)
00174  */
00175 /* 0x1E INT_STATUS_2
00176  *      bit[7] tap_sign
00177  *      bit[6] tap_first_z
00178  *      bit[5] tap_first_y
00179  *      bit[4] tap_first_x
00180  *      bit[3] anym_sign
00181  *      bit[2] anym_first_z
00182  *      bit[1] anym_first_y
00183  *      bit[0] anym_first_x
00184  */
00185 /* 0x1F INT_STATUS_3
00186  *      bit[7] flat
00187  *      bit[6] orient_2
00188  *      bit[5] orient_1
00189  *      bit[4] orient_0
00190  *      bit[3] high_sign
00191  *      bit[2] high_first_z
00192  *      bit[1] high_first_y
00193  *      bit[0] high_first_x
00194  */
00195 /* 0x20 TEMPERATURE_0 temperature[7:0] */
00196 /* 0x21 TEMPERATURE_1 reset value = 0x80 temperature[15:8] */
00197 /* 0x22 FIFO_LENGTH_0 fifo_byte_counter[7:0] */
00198 /* 0x23 FIFO_LENGTH_1
00199  *      bit[7:3] (reserved)
00200  *      bit[2:0] fifo_byte_counter[10:8] 
00201  */
00202 /* 0x40 ACC_CONF reset value 0x28
00203  *      bit[7]   acc_us
00204  *      bit[6:4] acc_bwp
00205  *      bit[3:0] acc_odr
00206  */
00207 /* 0x41 ACC_RANGE reset value = 0x03
00208  *      bit[7:4] (reserved)
00209  *      bit[3:0] acc_range
00210  */
00211 /* 0x42 GYR_CONF reset value = 0x28
00212  *      bit[7:6] (reserved)
00213  *      bit[5:4] gyr_bwp
00214  *      bit[3:0] gyr_odr
00215  */
00216 /* 0x43 GYR_RANGE
00217  *      bit[7:3] (reserved)
00218  *      bit[2:0] gyr_range
00219  */
00220 /* 0x44 MAG_CONF reset value = 0x0B
00221  *      bit[7:4] (reserved)
00222  *      bit[3:0] mag_odr
00223  */
00224 /* 0x45 FIFO_DOWNS reset value = 0x88
00225  *      bit[7] acc_fifo_filt_data
00226  *      bit[6:4] acc_fifo_downs
00227  *      bit[3] gyr_fifo_filt_data
00228  *      bit[2:0] gyr_fifo_downs
00229  */
00230 /* 0x46 FIFO_CONFIG_0 reset value = 0x80
00231  *      bit[7:0] fifo_water_mark
00232  */
00233 /* 0x47 FIFO_CONFIG_1 reset value = 0x10
00234  *      bit[7] fifo_gyr_en
00235  *      bit[6] fifo_acc_en
00236  *      bit[5] fifo_mag_en
00237  *      bit[4] fifo_header_en
00238  *      bit[3] fifo_tag_int1_en
00239  *      bit[2] fifo_tag_int2_en
00240  *      bit[1] fifo_time_en
00241  *      bit[0] (reserved)
00242  */
00243 /* 0x4B MAG_IF_0 reset value = 0x20
00244  *      bit[7:1] i2c_device_addr
00245  */
00246 /* 0x4C MAG_IF_1 reset value = 0x80
00247  *      bit[7] mag_manual_en
00248  *      bit[6] (reserved)
00249  *      bit[5:2] mag_offset
00250  *      bit[1:0] mag_rd_burst
00251  */
00252 /* 0x4D MAG_IF_2 reset value = 0x42
00253  *      bit[7:0] read_addr
00254  */
00255 /* 0x4E MAG_IF_3 reset value = 0x4C
00256  *      bit[7:0] write_addr
00257  */
00258 /* 0x4F MAG_IF_4 bit[7:0] write data */
00259 /* 0x50 INT_EN_0 
00260  *      bit[7] int_flat_en
00261  *      bit[6] int_orient_en
00262  *      bit[5] int_s_tap_en
00263  *      bit[4] int_d_tap_en
00264  *      bit[3] (reserved)
00265  *      bit[2] int_anymo_z_en
00266  *      bit[1] int_anymo_y_en
00267  *      bit[0] int_anymo_x_en
00268  */
00269 /* 0x51 INT_EN_1
00270  *      bit[7] (reserved)
00271  *      bit[6] int_fwm_en
00272  *      bit[5] int_ffull_en
00273  *      bit[4] int_drdy_en
00274  *      bit[3] int_low_en
00275  *      bit[2] int_highg_z_en
00276  *      bit[1] int_highg_y_en
00277  *      bit[0] int_highg_x_en
00278  */
00279 /* 0x52 INT_EN_2
00280  *      bit[7:4] (reserved)
00281  *      bit[3] int_step_det_en
00282  *      bit[2] int_nomoz_en
00283  *      bit[1] int_nomoy_en
00284  *      bit[0] int_nomox_en
00285  */
00286 /* 0x53 INT_OUT_CTRL
00287  *      bit[7] int2_output_en
00288  *      bit[6] int2_od
00289  *      bit[5] int2_lvl
00290  *      bit[4] int2_edge_ctrl
00291  *      bit[3] int1_output_en
00292  *      bit[2] int1_od
00293  *      bit[1] int1_lvl
00294  *      bit[0] itn1_edge_ctrl
00295  */
00296 /* 0x54 INT_LATCH
00297  *      bit[7:6] (reserved)
00298  *      bit[5] int2_input_en
00299  *      bit[4] int1_input_en
00300  *      bit[3:0] int_latch
00301  */
00302 /* 0x55 INT_MAP_0
00303  *      bit[7] int1_flat
00304  *      bit[6] int1_orient
00305  *      bit[5] int1_s_tap
00306  *      bit[4] int1_d_tap
00307  *      bit[3] int1_nomotion
00308  *      bit[2] int1_anymotion
00309  *      bit[1] int1_highg
00310  *      bit[0] int1_lowg_step
00311  */
00312 /* 0x56 INT_MAP_1
00313  *      bit[7] int1_drdy
00314  *      bit[6] int1_fwm
00315  *      bit[5] int1_ffull
00316  *      bit[4] int1_pmu_trig
00317  *      bit[3] int2_drdy
00318  *      bit[2] itn2_fwm
00319  *      bit[1] int2_ffull
00320  *      bit[0] int2_pmu_trig
00321  */
00322 /* 0x57 INT_MAP_2
00323  *      bit[7] int2_flat
00324  *      bit[6] int2_orient
00325  *      bit[5] int2_s_tap
00326  *      bit[4] int2_d_tap
00327  *      bit[3] int2_nomotion
00328  *      bit[2] int2_anymotion
00329  *      bit[1] int2_highg
00330  *      bit[0] int2_lowg_steop
00331  */
00332 /* 0x58 INT_DATA_0
00333  *      bit[7] int_low_high_src
00334  *      bit[6:4] (reserved)
00335  *      bit[3] int_tap_src
00336  *      bit[2:0] (reserved)
00337  */
00338 /* 0x59 INT_DATA_1
00339  *      bit[7] int_motion_src
00340  *      bit[6:0] (reseved)
00341  */
00342 /* 0x5A INT_LOWHIGH_0 reset value = 0x07
00343  *      bit[7:0] int_low_dur
00344  */
00345 /* 0x5B INT_LOWHIGH_1 reset value = 0x30
00346  *      bit[7:0] int_low_th
00347  */
00348 /* 0x5C INT_LOWHIGH_2 reset value = 0x81
00349  *      bit[7:6] int_high_hy
00350  *      bit[5:3] (reserved)
00351  *      bit[2]   int_low_mode
00352  *      bit[1:0] int_low_hy
00353  */
00354 /* 0x5D INT_LOWHIGH_3 reset value = 0x0B
00355  *      bit[7:0] int_high_dur
00356  */
00357 /* 0x5E INT_LOWHIGH_4 reset value = 0xC0
00358  *      bit[7:0] int_high_th
00359  */
00360 /* 0x5F INT_MOTION_0 
00361  *      bit[7:2] int_slo_nomo_dur
00362  *      bit[1:0] int_anym_dur
00363  */
00364 /* 0x60 INT_MOTION_1 reset value = 0x14
00365  *      bit[7:0] int_anymo_th
00366  */
00367 /* 0x61 INT_MOTION_2 reset value = 0x14
00368  *      bit[7:0] int_slo_nomo_th
00369  */
00370 /* 0x62 INT_MOTION_3 reset value = 0x24
00371  *      bit[7:6] (reserved)
00372  *      bit[5:4] int_sig_mot_proof
00373  */
00374 /* 0x63 INT_TAP_0 reset value = 0x04
00375  *      bit[7] int_tap_quiet
00376  *      bit[6] int_tap_shock
00377  *      bit[5:3] (reserved)
00378  *      bit[2:0] int_tap_dur
00379  */
00380 /* 0x64 INT_TAP_1 reset value = 0x0A
00381  *      bit[7:5] (reserved)
00382  *      bit[4:0] int_tap_th
00383  */
00384 /* 0x65 INT_ORIENT_0 reset value = 0x18
00385  *      bit[7:4] int_orient_hy
00386  *      bit[3:2] int_orient_blocking
00387  *      bit[1:0] int_orient_mode
00388  */
00389 /* 0x66 INT_ORIENT_1 reset value = 0x48
00390  *      bit[7] int_orient_axes_ex
00391  *      bit[6] int_orient_ud_en
00392  *      bit[5:0] int_orient_theta
00393  */
00394 /* 0x67 INT_FLAT_0 reset value = 0x08
00395  *      bit[7:6] (reserved)
00396  *      bit[5:0] int_flat_theta
00397  */
00398 /* 0x68 INT_FLAT_1 reset value = 0x11
00399  *      bit[7:6] (reserved)
00400  *      bit[5:4] int_flat_hold
00401  *      bit[3:0] int_flat_hy
00402  */
00403 /* 0x69 FOC_CONF 
00404  *      bit[7] (reserved)
00405  *      bit[6] foc_gyr_en
00406  *      bit[5:4] foc_acc_x
00407  *      bit[3:2] foc_acc_y
00408  *      bit[1:0] foc_acc_z
00409  */
00410 /* 0x6A CONF
00411  *      bit[7:2] (reserved)
00412  *      bit[1]  nvm_prog_en
00413  *      bit[0] (reserved)
00414  */
00415 /* 0x6B IF_CONF
00416  *      bit[7:6] (reserved)
00417  *      bit[5:4] if_mode
00418  *      bit[3:1] (reserved)
00419  *      bit[0] spi3
00420  */
00421 /* 0x6C PMU_TRIGGER
00422  *      bit[7] (reserved)
00423  *      bit[6] wakeup_int
00424  *      bit[5] gyr_sleep_state
00425  *      bit[4:3] gyr_wakeup_trigger
00426  *      bit[2:0] gyr_sleep_trigger
00427  */
00428 /* 0x6D SELF_TEST
00429  *      bit[7:5] (reserved)
00430  *      bit[4] gyr_self_test_enable
00431  *      bit[3] acc_self_test_amp
00432  *      bit[2] acc_self_test_sign
00433  *      bit[1:0] acc_self_test_enable
00434  */
00435 /* 0x70 NV_CONF
00436  *      bit[7:4] (reserved)
00437  *      bit[3] u_spare_0
00438  *      bit[2] i2c_wdt_en
00439  *      bit[1] i2c_wdt_sel
00440  *      bit[0] i2c_spi_en
00441  */
00442 /* 0x71 OFFSET_0 bit[7:0] off_acc_x */
00443 /* 0x72 OFFSET_1 bit[7:0] off_acc_y */
00444 /* 0x73 OFFSET_2 bit[7:0] off_acc_z */
00445 /* 0x74 OFFSET_3 bit[7:0] off_gyr_x_7_0 */
00446 /* 0x75 OFFSET_4 bit[7:0] off_gyr_y_7_0 */
00447 /* 0x76 OFFSET_5 bit[7:0] off_gyr_z_7_0 */
00448 /* 0x77 OFFSET_6
00449  *      bit[7] gyr_off_en
00450  *      bit[6] acc_off_en
00451  *      bit[5:4] off_gyr_Z_9_8
00452  *      bit[3:2] off_gyr_y_9_8
00453  *      bit[1:0] off_gyr_x_9_8
00454  */
00455 /* 0x78 STEP_CNT_0 bit[7:0] step_cnt_7_0 */
00456 /* 0x79 STEP_CNT_1 bit[7:0] step_cnt_15_8 */
00457 /* 0x7A STEP_CONF_0 reset value = 0x15
00458  *      bit[7:5] (alpha)
00459  *      bit[4:3] min_threshold
00460  *      bit[2:0] steptime_min
00461  */
00462 /* 0x7B STEP_CONF_1 reset value = 0x03
00463  *      bit[7:4] (reserved)
00464  *      bit[3] step_cnt_en
00465  *      bit[2:0] min_step_buf
00466  */
00467 /* 0x7E CMD bit[7:0] cmd */
00468 
00469  
00470  
00471 
00472 BMI160::BMI160(PinName sda, PinName scl, int addr) 
00473 {
00474     m_i2c = new I2C(sda, scl) ;
00475     m_addr = (addr << 1) ;
00476     m_spi = 0 ;
00477     m_cs = 0 ;
00478     init() ;
00479 }
00480 
00481 BMI160::BMI160(PinName sck, PinName miso, PinName mosi, PinName cs) 
00482 {
00483     m_cs = new DigitalOut(cs, 1) ;
00484     m_spi = new SPI(mosi, miso, sck) ;
00485     m_spi->format(8, 3) ;
00486     m_i2c = 0 ;
00487     m_addr = 0 ;
00488     init() ;
00489 }
00490 
00491 BMI160::~BMI160() 
00492 {
00493     if (m_spi) { 
00494         delete m_spi ;
00495         delete m_cs ; 
00496     }
00497     if (m_i2c) { 
00498         delete m_i2c ; 
00499         m_addr = 0 ;
00500     }
00501 }
00502 
00503 void BMI160::i2c_readRegs(int addr, uint8_t *data, int len) 
00504 {
00505     char t[1] = {addr} ;
00506     m_i2c->write(m_addr, t, 1, true) ;
00507     m_i2c->read(m_addr, (char*)data, len) ;
00508 }
00509 
00510 void BMI160::i2c_writeRegs(uint8_t *data, int len) 
00511 {
00512    m_i2c->write(m_addr, (char *)data, len) ;
00513 }
00514 
00515 void BMI160::spi_readRegs(int addr, uint8_t *data, int len) 
00516 {
00517     *m_cs = 0 ;
00518     m_spi->write(addr | 0x80) ;
00519     for (int i = 0 ; i < len ; i++ ) {    
00520       data[i] = m_spi->write((addr+i)|0x80) ; 
00521     } 
00522     m_spi->write(0x00) ; // to terminate read mode
00523     *m_cs = 1 ;
00524 }
00525 
00526 void BMI160::spi_writeRegs(uint8_t *data, int len) 
00527 {
00528    *m_cs = 0 ;
00529    for (int i = 0 ; i < len-1 ; i++ ) {
00530       m_spi->write((data[0]+i)^0x80) ; /* register address */
00531       m_spi->write(data[i+1]) ; /* data to write */
00532 
00533    }
00534    *m_cs = 1 ;
00535 }
00536 
00537 void BMI160::readRegs(int addr, uint8_t *data, int len) 
00538 {
00539     if (m_spi) {
00540         spi_readRegs(addr, data, len) ;
00541     } else if (m_i2c) {
00542         i2c_readRegs(addr, data, len) ;
00543     }
00544 }
00545 
00546 void BMI160::writeRegs(uint8_t *data, int len) 
00547 {
00548     if (m_spi) {
00549         spi_writeRegs(data, len) ;
00550     } else if (m_i2c) {
00551         i2c_writeRegs(data, len) ;
00552     }
00553 }
00554 
00555 void BMI160::init(void)
00556 {
00557     acc_range = getAccRange() ;
00558     gyr_range = getGyrRange() ;
00559 }
00560 
00561 
00562 void BMI160::setCMD(uint8_t cmd) 
00563 {
00564     uint8_t data[2] ;
00565     data[0] = REG_CMD ;
00566     data[1] = cmd ;
00567     writeRegs(data, 2) ;
00568 }
00569 
00570 uint8_t BMI160::getStatus(void)
00571 {
00572   uint8_t status ;
00573   readRegs(REG_STATUS, &status, 1) ;
00574   return(status) ;
00575 }
00576   
00577 uint8_t BMI160::getChipID(void) 
00578 {
00579     uint8_t data ;
00580     readRegs(REG_CHIP_ID, &data, 1) ;
00581     return( data ) ;
00582 }
00583 
00584 uint8_t BMI160::getAccRange(void)
00585 {
00586     uint8_t data = 0 ;
00587     uint8_t range = 0 ;
00588     readRegs(REG_ACC_RANGE, &data, 1) ;
00589     switch(data & 0x0F) {
00590     case 3: /* +/- 2g */
00591         range = 2 ; 
00592         break ;
00593     case 5: /* +/- 4g */
00594         range = 4 ;
00595         break ;
00596     case 8: /* +/- 8g */
00597         range = 8 ;
00598         break ;
00599     default:
00600         printf("illegal Acc Range %X detected\n", data & 0x0F) ;
00601         break ;
00602     }
00603     acc_range = range ;
00604     return(range) ;
00605 }
00606 
00607 int16_t BMI160::getGyrRange(void)
00608 {
00609     uint8_t data = 0 ;
00610     int16_t range = 0 ;
00611     readRegs(REG_GYR_RANGE, &data, 1) ;
00612     switch(data & 0x07) {
00613     case 0:
00614         range = 2000 ;
00615         break ;
00616     case 1:
00617         range = 1000 ;
00618         break ;
00619     case 2:
00620         range = 500 ;
00621         break ;
00622     case 3:
00623         range = 250 ;
00624         break ;
00625     case 4:
00626         range = 125 ;
00627         break ;
00628     default:
00629         printf("illegal Gyr Range %d detected\n", data & 0x07) ;
00630         break ;
00631     }
00632     gyr_range = range ;
00633     return(range) ;
00634 }
00635 
00636 int16_t BMI160::getAccRawX(void) 
00637 {
00638     uint8_t data[2] ;
00639     int16_t value = 0 ;
00640     readRegs(REG_DATA_14, data, 2) ;
00641     value = (data[1] << 8) | data[0] ;
00642     return( value ) ;
00643 }
00644 
00645 int16_t BMI160::getAccRawY(void) 
00646 {
00647     uint8_t data[2] ;
00648     int16_t value = 0 ;
00649     readRegs(REG_DATA_16, data, 2) ;
00650     value = (data[1] << 8) | data[0] ;
00651     return( value ) ;
00652 }
00653 
00654 int16_t BMI160::getAccRawZ(void) 
00655 {
00656     uint8_t data[2] ;
00657     int16_t value = 0 ;
00658     readRegs(REG_DATA_18, data, 2) ;
00659     value = (data[1] << 8) | data[0] ;
00660     return( value ) ;
00661 }
00662 
00663 void BMI160::getAccRaw(int16_t *value)
00664 {
00665     uint8_t data[6] ;
00666     readRegs(REG_DATA_14, data, 6) ;
00667     value[0] = (data[1] << 8) | data[0] ;
00668     value[1] = (data[3] << 8) | data[2] ;
00669     value[2] = (data[5] << 8) | data[4] ;
00670 } 
00671 
00672 int16_t BMI160::getGyrRawX(void) 
00673 {
00674     uint8_t data[2] ;
00675     int16_t value = 0 ;
00676     readRegs(REG_DATA_8, data, 2) ;
00677     value = (data[1] << 8) | data[0] ;
00678     return( value ) ;
00679 }
00680 
00681 int16_t BMI160::getGyrRawY(void) 
00682 {
00683     uint8_t data[2] ;
00684     int16_t value = 0 ;
00685     readRegs(REG_DATA_10, data, 2) ;
00686     value = (data[1] << 8) | data[0] ;
00687     return( value ) ;
00688 }
00689 
00690 int16_t BMI160::getGyrRawZ(void) 
00691 {
00692     uint8_t data[2] ;
00693     int16_t value = 0 ;
00694     readRegs(REG_DATA_12, data, 2) ;
00695     value = (data[1] << 8) | data[0] ;
00696     return( value ) ;
00697 }
00698 
00699 void BMI160::getGyrRaw(int16_t *value)
00700 {
00701     uint8_t data[6] ;
00702     readRegs(REG_DATA_8, data, 6) ;
00703     value[0] = (data[1] << 8) | data[0] ;
00704     value[1] = (data[3] << 8) | data[2] ;
00705     value[2] = (data[5] << 8) | data[4] ;
00706 } 
00707 
00708 float BMI160::getAccX(void)
00709 {
00710     float value ;
00711     value = acc_range * getAccRawX() / FLOAT_MAX_16BIT ;
00712     return(value) ;
00713 }
00714 
00715 float BMI160::getAccY(void)
00716 {
00717     float value ;
00718     value = acc_range * getAccRawY() / FLOAT_MAX_16BIT ;
00719     return(value) ;
00720 }
00721 
00722 float BMI160::getAccZ(void)
00723 {
00724     float value ;
00725     value = acc_range * getAccRawZ() / FLOAT_MAX_16BIT ;
00726     return(value) ;
00727 }
00728 
00729 void BMI160::getAcc(float *value)
00730 {
00731     int16_t data[3] ;
00732     getAccRaw(data) ;
00733     value[0] = acc_range * data[0] / FLOAT_MAX_16BIT ;
00734     value[1] = acc_range * data[1] / FLOAT_MAX_16BIT ;
00735     value[2] = acc_range * data[2] / FLOAT_MAX_16BIT ;
00736 }
00737 
00738 float BMI160::getGyrX(void)
00739 {
00740     float value ;
00741     value = gyr_range * getGyrRawX() / FLOAT_MAX_16BIT ;
00742     return(value) ;
00743 }
00744 
00745 float BMI160::getGyrY(void)
00746 {
00747     float value ;
00748     value = gyr_range * getGyrRawY() / FLOAT_MAX_16BIT ;
00749     return(value) ;
00750 }
00751 
00752 float BMI160::getGyrZ(void)
00753 {
00754     float value ;
00755     value = gyr_range * getGyrRawZ() / FLOAT_MAX_16BIT ;
00756     return(value) ;
00757 }
00758 
00759 void BMI160::getGyr(float *value)
00760 {
00761     int16_t data[3] ;
00762     getGyrRaw(data) ;
00763     value[0] = gyr_range * data[0] / FLOAT_MAX_16BIT ;
00764     value[1] = gyr_range * data[1] / FLOAT_MAX_16BIT ;
00765     value[2] = gyr_range * data[2] / FLOAT_MAX_16BIT ;
00766 }