altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Xtra_Sensors/TFMini_i2c.cpp@4:dc844fde64d7, 2019-11-22 (annotated)

Committer:
Kiwicjam
Date:
Fri Nov 22 08:40:26 2019 +0000
Revision:
4:dc844fde64d7
Parent:
2:e7874762cc25 
Workin set, not running,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 2:e7874762cc25 1 #include "TFMini_i2c.h"
altb2 2:e7874762cc25 2
altb2 2:e7874762cc25 3 // constructor
altb2 2:e7874762cc25 4 TFMini_i2c::TFMini_i2c(I2C * i2c, const uint8_t& addr):i2c_(i2c), addr_( addr )
altb2 2:e7874762cc25 5 {
altb2 2:e7874762cc25 6 waitTime_ms_ = 10;
altb2 2:e7874762cc25 7 isInitialized_ = false;
altb2 2:e7874762cc25 8 if( !isInitialized_ )
altb2 2:e7874762cc25 9 initialize();
altb2 2:e7874762cc25 10 }
altb2 2:e7874762cc25 11
altb2 2:e7874762cc25 12 // deconstructor
altb2 2:e7874762cc25 13 TFMini_i2c::~TFMini_i2c() {}
altb2 2:e7874762cc25 14
altb2 2:e7874762cc25 15 TFMini_i2c::operator float()
altb2 2:e7874762cc25 16 {
altb2 2:e7874762cc25 17 uint16_t dist(0), stren(0);
altb2 2:e7874762cc25 18 uint8_t mod(0);
altb2 2:e7874762cc25 19 char distUnit[1] = {0};
altb2 2:e7874762cc25 20
altb2 2:e7874762cc25 21 readRegister(TFMINI_UNIT_DIST_H, TFMINI_UNIT_DIST_L, distUnit, 1);
altb2 2:e7874762cc25 22 readMeasurement( &dist, &stren, &mod );
altb2 2:e7874762cc25 23
altb2 2:e7874762cc25 24 switch(static_cast<uint8_t>(distUnit[0]) ) {
altb2 2:e7874762cc25 25 case(0x00):
altb2 2:e7874762cc25 26 return(dist/1000.0f);
altb2 2:e7874762cc25 27 case(0x01):
altb2 2:e7874762cc25 28 return(dist/100.0f);
altb2 2:e7874762cc25 29 default:
altb2 2:e7874762cc25 30 return(0xffffffff);
altb2 2:e7874762cc25 31 }
altb2 2:e7874762cc25 32 }
altb2 2:e7874762cc25 33 // Initialization
altb2 2:e7874762cc25 34 // Write here the default configuration registers
altb2 2:e7874762cc25 35 //
altb2 2:e7874762cc25 36
altb2 2:e7874762cc25 37 void TFMini_i2c::initialize()
altb2 2:e7874762cc25 38 {
altb2 2:e7874762cc25 39 isInitialized_ = false;
altb2 2:e7874762cc25 40
altb2 2:e7874762cc25 41 reset();
altb2 2:e7874762cc25 42 setRangingGearMode(FIX,MIDDLE);
altb2 2:e7874762cc25 43 setDistanceUnit(MILLIMETER);
altb2 2:e7874762cc25 44
altb2 2:e7874762cc25 45 isInitialized_ = true;
altb2 2:e7874762cc25 46
altb2 2:e7874762cc25 47 return;
altb2 2:e7874762cc25 48 }
altb2 2:e7874762cc25 49 // Reading Data from TFmini-I2C
altb2 2:e7874762cc25 50
altb2 2:e7874762cc25 51 void TFMini_i2c::readRegister( const char &reg_h, const char &reg_l, char * bufIn, const uint8_t &len )
altb2 2:e7874762cc25 52 {
altb2 2:e7874762cc25 53 read( reg_h, reg_l, bufIn, len );
altb2 2:e7874762cc25 54 }
altb2 2:e7874762cc25 55
altb2 2:e7874762cc25 56 void TFMini_i2c::readMeasurement( uint16_t *dist, uint16_t *stren, uint8_t *mode )
altb2 2:e7874762cc25 57 {
altb2 2:e7874762cc25 58 char dataIn[7] = {0};
altb2 2:e7874762cc25 59
altb2 2:e7874762cc25 60 read( TFMINI_READ_DATA_REG_H, TFMINI_READ_DATA_REG_L, dataIn, 7 );
altb2 2:e7874762cc25 61
altb2 2:e7874762cc25 62 *dist = static_cast<uint16_t>( dataIn[2] | ( dataIn[3] << 8 ) );
altb2 2:e7874762cc25 63 *stren = static_cast<uint16_t>( dataIn[4] | ( dataIn[5] << 8 ) );
altb2 2:e7874762cc25 64 *mode = static_cast<uint8_t>( dataIn[6] );
altb2 2:e7874762cc25 65
altb2 2:e7874762cc25 66 }
altb2 2:e7874762cc25 67
altb2 2:e7874762cc25 68
altb2 2:e7874762cc25 69 // General Parameter Configuration
altb2 2:e7874762cc25 70
altb2 2:e7874762cc25 71 void TFMini_i2c::setRangingGearMode( detection_pattern_t mode, const ranging_gear_mode_t dist )
altb2 2:e7874762cc25 72 {
altb2 2:e7874762cc25 73 uint16_t regVal( mode << 8 | dist );
altb2 2:e7874762cc25 74
altb2 2:e7874762cc25 75 write16( TFMINI_RNG_GEAR_H, TFMINI_RNG_GEAR_L, regVal );
altb2 2:e7874762cc25 76 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 77 }
altb2 2:e7874762cc25 78
altb2 2:e7874762cc25 79
altb2 2:e7874762cc25 80 void TFMini_i2c::setRangeLimit( setting_range_limit_t mode, const uint16_t & threshold )
altb2 2:e7874762cc25 81 {
altb2 2:e7874762cc25 82
altb2 2:e7874762cc25 83 write16( TFMINI_SET_RNG_LIM_H, TFMINI_SET_RNG_LIM_L, (uint16_t) mode );
altb2 2:e7874762cc25 84 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 85 write16( TFMINI_RNG_OUT_LIM_THR_H, TFMINI_RNG_OUT_LIM_THR_L, threshold );
altb2 2:e7874762cc25 86 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 87 }
altb2 2:e7874762cc25 88
altb2 2:e7874762cc25 89
altb2 2:e7874762cc25 90 void TFMini_i2c::setLowerLimitOfSignalStrengthThreshold( uint16_t & threshold)
altb2 2:e7874762cc25 91 {
altb2 2:e7874762cc25 92 write16(TFMINI_STREN_THR_LOW_H, TFMINI_STREN_THR_LOW_L, threshold);
altb2 2:e7874762cc25 93 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 94 }
altb2 2:e7874762cc25 95
altb2 2:e7874762cc25 96
altb2 2:e7874762cc25 97 void TFMini_i2c::setUpperLimitOfSignalStrengthThreshold( uint16_t & threshold)
altb2 2:e7874762cc25 98 {
altb2 2:e7874762cc25 99 write16(TFMINI_STREN_THR_UPP_H, TFMINI_STREN_THR_UPP_L, threshold);
altb2 2:e7874762cc25 100 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 101 }
altb2 2:e7874762cc25 102
altb2 2:e7874762cc25 103
altb2 2:e7874762cc25 104 void TFMini_i2c::setOutputValueOfSignalStrengthThresholdAtHighestPoint( uint16_t & val)
altb2 2:e7874762cc25 105 {
altb2 2:e7874762cc25 106 write16(TFMINI_OUT_VAL_SGN_STREN_H, TFMINI_OUT_VAL_SGN_STREN_L, val);
altb2 2:e7874762cc25 107 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 108 }
altb2 2:e7874762cc25 109
altb2 2:e7874762cc25 110
altb2 2:e7874762cc25 111 void TFMini_i2c::setDistanceUnit( unit_distance_t unit )
altb2 2:e7874762cc25 112 {
altb2 2:e7874762cc25 113 write8(TFMINI_UNIT_DIST_H, TFMINI_UNIT_DIST_L, unit);
altb2 2:e7874762cc25 114 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 115 }
altb2 2:e7874762cc25 116
altb2 2:e7874762cc25 117
altb2 2:e7874762cc25 118 // Special Parameter Configuration
altb2 2:e7874762cc25 119
altb2 2:e7874762cc25 120 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 121 //
altb2 2:e7874762cc25 122 // Set a new i2c-address between 0x10 - 0x78
altb2 2:e7874762cc25 123 // After setting a new address the power of the TFMini must be shut of.
altb2 2:e7874762cc25 124 // This function is maintenance. It is not possible to change the address
altb2 2:e7874762cc25 125 // in process yet!!!
altb2 2:e7874762cc25 126 //
altb2 2:e7874762cc25 127 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 128
altb2 2:e7874762cc25 129 void TFMini_i2c::setSlaveAddress( const uint8_t &addr )
altb2 2:e7874762cc25 130 {
altb2 2:e7874762cc25 131 char addrW( addr << 1 | 0 ), len( 0x02 );
altb2 2:e7874762cc25 132 char bufW[] = { addrW, TFMINI_SLAVE_ADDR_H, TFMINI_SLAVE_ADDR_L, len, addr, 0x00 };
altb2 2:e7874762cc25 133
altb2 2:e7874762cc25 134 write8( TFMINI_SLAVE_ADDR_H, TFMINI_SLAVE_ADDR_L, addr );
altb2 2:e7874762cc25 135 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 136 setSlaveAddrActive();
altb2 2:e7874762cc25 137 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 138 addr_ = addr;
altb2 2:e7874762cc25 139 }
altb2 2:e7874762cc25 140
altb2 2:e7874762cc25 141 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 142 //
altb2 2:e7874762cc25 143 // There are two trigger modes for the measurement of TFmini-I²C. The default
altb2 2:e7874762cc25 144 // one is the internaltimer trigger by the timer with one measurement per 10ms.
altb2 2:e7874762cc25 145 // The user may modify this mode into the external command trigger mode which
altb2 2:e7874762cc25 146 // allows the TFmini-I²C to start the distance measurement by an external
altb2 2:e7874762cc25 147 // trigger instruction. Please note that the trigger frequency of TFmini-I²C
altb2 2:e7874762cc25 148 // should in no way be more than 100Hz as the maximum.
altb2 2:e7874762cc25 149 //
altb2 2:e7874762cc25 150 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 151
altb2 2:e7874762cc25 152 void TFMini_i2c::setTriggerMode( trigger_mode_t mode )
altb2 2:e7874762cc25 153 {
altb2 2:e7874762cc25 154 write8( TFMINI_TRIG_MOD_H, TFMINI_TRIG_MOD_L, mode );
altb2 2:e7874762cc25 155 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 156 }
altb2 2:e7874762cc25 157
altb2 2:e7874762cc25 158 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 159 //
altb2 2:e7874762cc25 160 // Use this Method to trigger Measurement. Works only if trigger mode
altb2 2:e7874762cc25 161 // is EXTERNAL.
altb2 2:e7874762cc25 162 //
altb2 2:e7874762cc25 163 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 164
altb2 2:e7874762cc25 165 void TFMini_i2c::externalCommandTrigger( void )
altb2 2:e7874762cc25 166 {
altb2 2:e7874762cc25 167 write8( TFMINI_EXT_COM_TRIG_REG_H, TFMINI_EXT_COM_TRIG_REG_L, 0x01 );
altb2 2:e7874762cc25 168 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 169 }
altb2 2:e7874762cc25 170
altb2 2:e7874762cc25 171 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 172 //
altb2 2:e7874762cc25 173 // Reset of default configuration. By sending such instruction, all adjustable
altb2 2:e7874762cc25 174 // configurations will bereset back to the default configurations (excluding
altb2 2:e7874762cc25 175 // slave address and trigger mode). Please use it withcaution.
altb2 2:e7874762cc25 176 //
altb2 2:e7874762cc25 177 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 178
altb2 2:e7874762cc25 179 void TFMini_i2c::reset( void )
altb2 2:e7874762cc25 180 {
altb2 2:e7874762cc25 181 write8( TFMINI_RESET_H, TFMINI_RESET_L, 0x02 );
altb2 2:e7874762cc25 182 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 183 }
altb2 2:e7874762cc25 184
altb2 2:e7874762cc25 185
altb2 2:e7874762cc25 186
altb2 2:e7874762cc25 187
altb2 2:e7874762cc25 188 // private
altb2 2:e7874762cc25 189 //
altb2 2:e7874762cc25 190 void TFMini_i2c::setSlaveAddrActive( void )
altb2 2:e7874762cc25 191 {
altb2 2:e7874762cc25 192 char bufW[] = { 0x06 };
altb2 2:e7874762cc25 193 i2c_ -> write( addr_ << 1, bufW, 1, false );
altb2 2:e7874762cc25 194 wait_ms( waitTime_ms_ );
altb2 2:e7874762cc25 195 }
altb2 2:e7874762cc25 196
altb2 2:e7874762cc25 197 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 198 //
altb2 2:e7874762cc25 199 // Write one byte at given register address. Though it is only possible to write
altb2 2:e7874762cc25 200 // two bytes at once, the neighbouring register will be read. The two bytes are
altb2 2:e7874762cc25 201 // concatenated. Then two bytes are written.
altb2 2:e7874762cc25 202 //
altb2 2:e7874762cc25 203 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 204
altb2 2:e7874762cc25 205 void TFMini_i2c::write8( const char & reg_h, const char & reg_l, const uint8_t & val )
altb2 2:e7874762cc25 206 {
altb2 2:e7874762cc25 207 // read the configuration at register address + 1 to not overwrite these settings
altb2 2:e7874762cc25 208 //
altb2 2:e7874762cc25 209 char bufR[2] = { 0 };
altb2 2:e7874762cc25 210 readRegister( reg_h, reg_l, bufR, 2 );
altb2 2:e7874762cc25 211 bufR[0] = val;
altb2 2:e7874762cc25 212
altb2 2:e7874762cc25 213 // write the two byte value
altb2 2:e7874762cc25 214 //
altb2 2:e7874762cc25 215 uint16_t val2bytes( bufR[1] << 8 | bufR[0] );
altb2 2:e7874762cc25 216
altb2 2:e7874762cc25 217 write16( reg_h, reg_l, val2bytes );
altb2 2:e7874762cc25 218 }
altb2 2:e7874762cc25 219
altb2 2:e7874762cc25 220 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 221 //
altb2 2:e7874762cc25 222 // Write 2 bytes at given register address
altb2 2:e7874762cc25 223 //
altb2 2:e7874762cc25 224 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 225
altb2 2:e7874762cc25 226 void TFMini_i2c::write16(const char &reg_h, const char &reg_l, const uint16_t &val )
altb2 2:e7874762cc25 227 {
altb2 2:e7874762cc25 228
altb2 2:e7874762cc25 229 char addrW( addr_ << 1 | 0 ), data0( val & 0xff ),
altb2 2:e7874762cc25 230 data1( ( val & ( 0xff << 8 ) ) >> 8 ), len( 0x02 );
altb2 2:e7874762cc25 231
altb2 2:e7874762cc25 232 char bufW[] = { addrW, reg_h, reg_l, len, data0, data1 };
altb2 2:e7874762cc25 233
altb2 2:e7874762cc25 234 i2c_ -> start( );
altb2 2:e7874762cc25 235 i2c_ -> write( bufW[0] );
altb2 2:e7874762cc25 236 i2c_ -> write( bufW[1] );
altb2 2:e7874762cc25 237 i2c_ -> write( bufW[2] );
altb2 2:e7874762cc25 238 i2c_ -> write( bufW[3] );
altb2 2:e7874762cc25 239
altb2 2:e7874762cc25 240 i2c_ -> start( );
altb2 2:e7874762cc25 241 i2c_ -> write( bufW[0] );
altb2 2:e7874762cc25 242 i2c_ -> write( bufW[4] );
altb2 2:e7874762cc25 243 i2c_ -> write( bufW[5] );
altb2 2:e7874762cc25 244 i2c_ -> stop( );
altb2 2:e7874762cc25 245 }
altb2 2:e7874762cc25 246
altb2 2:e7874762cc25 247 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 248 //
altb2 2:e7874762cc25 249 // Read a define number of bytes (len) at the a given register address. The
altb2 2:e7874762cc25 250 // incoming bytes are stored at the address of bufIn.
altb2 2:e7874762cc25 251 //
altb2 2:e7874762cc25 252 ////////////////////////////////////////////////////////////////////////////////
altb2 2:e7874762cc25 253
altb2 2:e7874762cc25 254 void TFMini_i2c::read( const char & reg_h, const char & reg_l, char * bufIn,
altb2 2:e7874762cc25 255 const uint8_t & len )
altb2 2:e7874762cc25 256 {
altb2 2:e7874762cc25 257 char bufOut[] = { reg_h, reg_l, (char)len };
altb2 2:e7874762cc25 258
altb2 2:e7874762cc25 259 i2c_ -> write( addr_ << 1, bufOut, 3, true );
altb2 2:e7874762cc25 260 i2c_ -> read( addr_ << 1, bufIn, len, false );
altb2 2:e7874762cc25 261 }