File tlv493d.h¶
Go to the documentation of this file
Source Code¶
#pragma once
#ifndef DSY_TLV493D_H
#define DSY_TLV493D_H
#define TLV493D_DEFAULTMODE POWERDOWNMODE
#define TLV493D_ADDRESS1 0x5E
#define TLV493D_ADDRESS2 0x1F
#define TLV493D_BUSIF_READSIZE 10
#define TLV493D_BUSIF_WRITESIZE 4
#define TLV493D_NUM_OF_REGMASKS 25
#define TLV493D_DEFAULTMODE POWERDOWNMODE
#define TLV493D_MEASUREMENT_READOUT 7
#define TLV493D_B_MULT 0.098f
#define TLV493D_TEMP_MULT 1.1
#define TLV493D_TEMP_OFFSET 315
#define REGMASK_READ 0
#define REGMASK_WRITE 1
namespace daisy
{
class Tlv493dI2CTransport
{
public:
Tlv493dI2CTransport() {}
~Tlv493dI2CTransport() {}
struct Config
{
I2CHandle::Config::Peripheral periph;
I2CHandle::Config::Speed speed;
Pin scl;
Pin sda;
uint8_t address;
Config()
{
periph = I2CHandle::Config::Peripheral::I2C_1;
speed = I2CHandle::Config::Speed::I2C_400KHZ;
scl = Pin(PORTB, 8);
sda = Pin(PORTB, 9);
address = TLV493D_ADDRESS1;
}
};
void Init(Config config)
{
config_ = config;
I2CHandle::Config i2c_config;
i2c_config.mode = I2CHandle::Config::Mode::I2C_MASTER;
i2c_config.periph = config_.periph;
i2c_config.speed = config_.speed;
i2c_config.pin_config.scl = config_.scl;
i2c_config.pin_config.sda = config_.sda;
err_ = false;
err_ |= I2CHandle::Result::OK != i2c_.Init(i2c_config);
}
// used for the weird reset sequence. This ends in an error but that's OK
void WriteAddress(uint8_t add, uint8_t *data, uint16_t size)
{
i2c_.TransmitBlocking(add, data, size, 10);
}
void Write(uint8_t *data, uint16_t size)
{
err_ |= I2CHandle::Result::OK
!= i2c_.TransmitBlocking(config_.address, data, size, 10);
}
void Read(uint8_t *data, uint16_t size)
{
err_ |= I2CHandle::Result::OK
!= i2c_.ReceiveBlocking(config_.address, data, size, 10);
}
bool GetError()
{
bool tmp = err_;
err_ = false;
return tmp;
}
uint8_t GetAddress() { return config_.address; }
private:
I2CHandle i2c_;
Config config_;
bool err_;
};
template <typename Transport>
class Tlv493d
{
public:
Tlv493d() {}
~Tlv493d() {}
enum Registers_e
{
R_BX1 = 0,
R_BX2,
R_BY1,
R_BY2,
R_BZ1,
R_BZ2,
R_TEMP1,
R_TEMP2,
R_FRAMECOUNTER,
R_CHANNEL,
R_POWERDOWNFLAG,
R_RES1,
R_RES2,
R_RES3,
W_PARITY,
W_ADDR,
W_INT,
W_FAST,
W_LOWPOWER,
W_TEMP_NEN,
W_LP_PERIOD,
W_PARITY_EN,
W_RES1,
W_RES2,
W_RES3
};
struct RegMask_t
{
uint8_t rw;
uint8_t byteAdress;
uint8_t bitMask;
uint8_t shift;
};
typedef struct
{
uint8_t fast;
uint8_t lp;
uint8_t lpPeriod;
uint16_t measurementTime;
} AccessMode_t;
enum AccessMode_e
{
POWERDOWNMODE = 0,
FASTMODE,
LOWPOWERMODE,
ULTRALOWPOWERMODE,
MASTERCONTROLLEDMODE,
};
const AccessMode_t accModes[5] = {
{0, 0, 0, 1000}, // POWERDOWNMODE
{1, 0, 0, 0}, // FASTMODE
{0, 1, 1, 10}, // LOWPOWERMODE
{0, 1, 0, 100}, // ULTRALOWPOWERMODE
{1, 1, 1, 10} // MASTERCONTROLLEDMODE
};
const RegMask_t RegMasks[25] = {
{REGMASK_READ, 0, 0xFF, 0}, // R_BX1
{REGMASK_READ, 4, 0xF0, 4}, // R_BX2
{REGMASK_READ, 1, 0xFF, 0}, // R_BY1
{REGMASK_READ, 4, 0x0F, 0}, // R_BY2
{REGMASK_READ, 2, 0xFF, 0}, // R_BZ1
{REGMASK_READ, 5, 0x0F, 0}, // R_BZ2
{REGMASK_READ, 3, 0xF0, 4}, // R_TEMP1
{REGMASK_READ, 6, 0xFF, 0}, // R_TEMP2
{REGMASK_READ, 3, 0x0C, 2}, // R_FRAMECOUNTER
{REGMASK_READ, 3, 0x03, 0}, // R_CHANNEL
{REGMASK_READ, 5, 0x10, 4}, // R_POWERDOWNFLAG
{REGMASK_READ, 7, 0x18, 3}, // R_RES1
{REGMASK_READ, 8, 0xFF, 0}, // R_RES2
{REGMASK_READ, 9, 0x1F, 0}, // R_RES3
{REGMASK_WRITE, 1, 0x80, 7}, // W_PARITY
{REGMASK_WRITE, 1, 0x60, 5}, // W_ADDR
{REGMASK_WRITE, 1, 0x04, 2}, // W_INT
{REGMASK_WRITE, 1, 0x02, 1}, // W_FAST
{REGMASK_WRITE, 1, 0x01, 0}, // W_LOWPOWER
{REGMASK_WRITE, 3, 0x80, 7}, // W_TEMP_EN
{REGMASK_WRITE, 3, 0x40, 6}, // W_LOWPOWER
{REGMASK_WRITE, 3, 0x20, 5}, // W_POWERDOWN
{REGMASK_WRITE, 1, 0x18, 3}, // W_RES1
{REGMASK_WRITE, 2, 0xFF, 0}, // W_RES2
{REGMASK_WRITE, 3, 0x1F, 0} // W_RES3
};
struct Config
{
typename Transport::Config transport_config;
Config() {}
};
enum Result
{
OK = 0,
ERR
};
Result Init(Config config)
{
config_ = config;
System::Delay(40); // 40ms startup delay
transport_.Init(config_.transport_config);
ResetSensor();
// get all register data from sensor
ReadOut();
// copy factory settings to write registers
SetRegBits(W_RES1, GetRegBits(R_RES1));
SetRegBits(W_RES2, GetRegBits(R_RES2));
SetRegBits(W_RES3, GetRegBits(R_RES3));
// enable parity detection
SetRegBits(W_PARITY_EN, 1);
// config sensor to lowpower mode
// also contains parity calculation and writeout to sensor
SetAccessMode(MASTERCONTROLLEDMODE);
prev_sample_period_ = System::GetNow();
return GetTransportErr();
}
void ReadOut() { transport_.Read(regReadData, TLV493D_BUSIF_READSIZE); }
void WriteOut() { transport_.Write(regWriteData, TLV493D_BUSIF_WRITESIZE); }
void SetRegBits(uint8_t regMaskIndex, uint8_t data)
{
if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
{
SetToRegs(&RegMasks[regMaskIndex], regWriteData, data);
}
}
uint8_t GetRegBits(uint8_t regMaskIndex)
{
if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
{
const RegMask_t *mask = &(RegMasks[regMaskIndex]);
if(mask->rw == REGMASK_READ)
{
return GetFromRegs(mask, regReadData);
}
else
{
return GetFromRegs(mask, regWriteData);
}
}
return 0;
}
void UpdateData()
{
uint32_t now = System::GetNow();
if(now - prev_sample_period_ >= GetMeasurementDelay())
{
prev_sample_period_ = now;
ReadOut();
// construct results from registers
mXdata = ConcatResults(GetRegBits(R_BX1), GetRegBits(R_BX2), true);
mYdata = ConcatResults(GetRegBits(R_BY1), GetRegBits(R_BY2), true);
mZdata = ConcatResults(GetRegBits(R_BZ1), GetRegBits(R_BZ2), true);
mTempdata = ConcatResults(
GetRegBits(R_TEMP1), GetRegBits(R_TEMP2), false);
// SetAccessMode(POWERDOWNMODE);
GetRegBits(R_CHANNEL);
mExpectedFrameCount = GetRegBits(R_FRAMECOUNTER) + 1;
}
}
void SetInterrupt(bool enable)
{
SetRegBits(W_INT, enable);
CalcParity();
WriteOut();
}
void EnableTemp(bool enable)
{
SetRegBits(W_TEMP_NEN, enable);
CalcParity();
WriteOut();
}
float GetX() { return static_cast<float>(mXdata) * TLV493D_B_MULT; }
float GetY() { return static_cast<float>(mYdata) * TLV493D_B_MULT; }
float GetZ() { return static_cast<float>(mZdata) * TLV493D_B_MULT; }
float GetTemp()
{
return static_cast<float>(mTempdata - TLV493D_TEMP_OFFSET)
* TLV493D_TEMP_MULT;
}
float GetAmount()
{
// sqrt(x^2 + y^2 + z^2)
return TLV493D_B_MULT
* sqrt(pow(static_cast<float>(mXdata), 2)
+ pow(static_cast<float>(mYdata), 2)
+ pow(static_cast<float>(mZdata), 2));
}
float GetAzimuth()
{
// arctan(y/x)
return atan2(static_cast<float>(mYdata), static_cast<float>(mXdata));
}
float GetPolar()
{
// arctan(z/(sqrt(x^2+y^2)))
return atan2(static_cast<float>(mZdata),
sqrt(pow(static_cast<float>(mXdata), 2)
+ pow(static_cast<float>(mYdata), 2)));
}
uint16_t GetMeasurementDelay() { return accModes[mMode].measurementTime; }
void SetAccessMode(AccessMode_e mode)
{
const AccessMode_t *modeConfig = &(accModes[mode]);
SetRegBits(W_FAST, modeConfig->fast);
SetRegBits(W_LOWPOWER, modeConfig->lp);
SetRegBits(W_LP_PERIOD, modeConfig->lpPeriod);
CalcParity();
WriteOut();
mMode = mode;
}
void CalcParity()
{
uint8_t i;
uint8_t y = 0x00;
// set parity bit to 1
// algorithm will calculate an even parity and replace this bit,
// so parity becomes odd
SetRegBits(W_PARITY, 1);
// combine array to one byte first
for(i = 0; i < TLV493D_BUSIF_WRITESIZE; i++)
{
y ^= regWriteData[i];
}
// combine all bits of this byte
y = y ^ (y >> 1);
y = y ^ (y >> 2);
y = y ^ (y >> 4);
// parity is in the LSB of y
SetRegBits(W_PARITY, y & 0x01);
}
int16_t ConcatResults(uint8_t upperByte, uint8_t lowerByte, bool upperFull)
{
//16-bit signed integer for 12-bit values of sensor
int16_t value = 0x0000;
if(upperFull)
{
value = upperByte << 8;
value |= (lowerByte & 0x0F) << 4;
}
else
{
value = (upperByte & 0x0F) << 12;
value |= lowerByte << 4;
}
value >>= 4; //shift left so that value is a signed 12 bit integer
return value;
}
private:
Config config_;
Transport transport_;
uint8_t regReadData[TLV493D_BUSIF_READSIZE];
uint8_t regWriteData[TLV493D_BUSIF_WRITESIZE];
int16_t mXdata, mYdata, mZdata, mTempdata, mExpectedFrameCount, mMode;
uint32_t prev_sample_period_;
Result GetTransportErr()
{
Result ret = transport_.GetError() ? ERR : OK;
return ret;
}
// internal function called by begin()
void ResetSensor()
{
uint8_t data;
if(transport_.GetAddress() == TLV493D_ADDRESS1)
{
// if the sensor shall be initialized with i2c address 0x1F
data = 0xFF;
}
else
{
// if the sensor shall be initialized with address 0x5E
data = 0x00;
}
// Write data to slave add 0x00
transport_.WriteAddress(0x00, &data, 1);
}
uint8_t GetFromRegs(const RegMask_t *mask, uint8_t *regData)
{
return (regData[mask->byteAdress] & mask->bitMask) >> mask->shift;
}
void SetToRegs(const RegMask_t *mask, uint8_t *regData, uint8_t toWrite)
{
if(mask->rw == REGMASK_WRITE)
{
uint8_t regValue = regData[mask->byteAdress];
regValue &= ~(mask->bitMask);
regValue |= (toWrite << mask->shift) & mask->bitMask;
regData[mask->byteAdress] = regValue;
}
}
};
using Tlv493dI2C = Tlv493d<Tlv493dI2CTransport>;
} // namespace daisy
#endif