mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-09-26 23:09:27 +08:00

- Modify TLV493d library to expose frame counter in order to check for lockup, and implement auto-reset in tlv_sensor in case of lockup - Implement MT6701 SimpleFOC sensor - Make display optional - Add optional LED, strain, ALS support - Connect ALS to LED and display brightness - Hardcoded strain gauge thresholds and haptic feedback
370 lines
9.9 KiB
C++
370 lines
9.9 KiB
C++
/**
|
|
* Tlv493d.cpp - Library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
|
|
*
|
|
* The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption
|
|
* in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is
|
|
* ideally suited for the measurement of 3D movements, linear movements and rotation movements.
|
|
*
|
|
* Have a look at the application note/reference manual for more information.
|
|
*
|
|
* Copyright (c) 2018 Infineon Technologies AG
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
|
|
* following conditions are met:
|
|
*
|
|
* Redistributions of source code must retain the above copyright notice, this list of conditions and the following
|
|
* disclaimer.
|
|
*
|
|
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided with the distribution.
|
|
*
|
|
* Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote
|
|
* products derived from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
|
|
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
|
* WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
|
|
#include "Tlv493d.h"
|
|
#include "./util/RegMask.h"
|
|
#include "./util/BusInterface2.h"
|
|
#include <math.h>
|
|
|
|
|
|
Tlv493d::Tlv493d(void)
|
|
{
|
|
mXdata = 0;
|
|
mYdata = 0;
|
|
mZdata = 0;
|
|
mTempdata = 0;
|
|
mExpectedFrameCount = 0x00;
|
|
}
|
|
|
|
|
|
Tlv493d::~Tlv493d(void)
|
|
{
|
|
end();
|
|
}
|
|
|
|
void Tlv493d::begin(void)
|
|
{
|
|
begin(Wire, TLV493D_ADDRESS1, true);
|
|
}
|
|
|
|
void Tlv493d::begin(TwoWire &bus)
|
|
{
|
|
begin(bus, TLV493D_ADDRESS1, true);
|
|
}
|
|
|
|
void Tlv493d::begin(TwoWire &bus, Tlv493d_Address_t slaveAddress, bool reset)
|
|
{
|
|
/**
|
|
* Workaround for kit2go vdd enable
|
|
*/
|
|
#ifdef TLV493D_A1B6_KIT2GO
|
|
pinMode(LED2, OUTPUT);
|
|
digitalWrite(LED2, HIGH);
|
|
delay(50);
|
|
#endif
|
|
|
|
initInterface(&mInterface, &bus, slaveAddress);
|
|
delay(TLV493D_STARTUPDELAY);
|
|
|
|
mInterface.bus->begin();
|
|
|
|
if(reset == true)
|
|
{
|
|
resetSensor(mInterface.adress);
|
|
}
|
|
|
|
// get all register data from sensor
|
|
tlv493d::readOut(&mInterface);
|
|
// copy factory settings to write registers
|
|
setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
|
|
setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
|
|
setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
|
|
// enable parity detection
|
|
setRegBits(tlv493d::W_PARITY_EN, 1);
|
|
// config sensor to lowpower mode
|
|
// also contains parity calculation and writeout to sensor
|
|
setAccessMode(TLV493D_DEFAULTMODE);
|
|
}
|
|
|
|
|
|
void Tlv493d::end(void)
|
|
{
|
|
disableInterrupt();
|
|
setAccessMode(POWERDOWNMODE);
|
|
}
|
|
|
|
|
|
bool Tlv493d::setAccessMode(AccessMode_e mode)
|
|
{
|
|
bool ret = BUS_ERROR;
|
|
const tlv493d::AccessMode_t *modeConfig = &(tlv493d::accModes[mode]);
|
|
setRegBits(tlv493d::W_FAST, modeConfig->fast);
|
|
setRegBits(tlv493d::W_LOWPOWER, modeConfig->lp);
|
|
setRegBits(tlv493d::W_LP_PERIOD, modeConfig->lpPeriod);
|
|
calcParity();
|
|
ret = tlv493d::writeOut(&mInterface);
|
|
if ( ret != BUS_ERROR )
|
|
{
|
|
mMode = mode;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
|
|
void Tlv493d::enableInterrupt(void)
|
|
{
|
|
setRegBits(tlv493d::W_INT, 1);
|
|
calcParity();
|
|
tlv493d::writeOut(&mInterface);
|
|
}
|
|
|
|
|
|
void Tlv493d::disableInterrupt(void)
|
|
{
|
|
setRegBits(tlv493d::W_INT, 0);
|
|
calcParity();
|
|
tlv493d::writeOut(&mInterface);
|
|
}
|
|
|
|
void Tlv493d::enableTemp(void)
|
|
{
|
|
setRegBits(tlv493d::W_TEMP_NEN, 0);
|
|
calcParity();
|
|
tlv493d::writeOut(&mInterface);
|
|
}
|
|
|
|
|
|
void Tlv493d::disableTemp(void)
|
|
{
|
|
setRegBits(tlv493d::W_TEMP_NEN, 1);
|
|
calcParity();
|
|
tlv493d::writeOut(&mInterface);
|
|
}
|
|
|
|
|
|
uint16_t Tlv493d::getMeasurementDelay(void)
|
|
{
|
|
return tlv493d::accModes[mMode].measurementTime;
|
|
}
|
|
|
|
|
|
Tlv493d_Error_t Tlv493d::updateData(void)
|
|
{
|
|
Tlv493d_Error_t ret = TLV493D_NO_ERROR;
|
|
// in POWERDOWNMODE, sensor has to be switched on for one measurement
|
|
uint8_t powerdown = 0;
|
|
if(mMode == POWERDOWNMODE)
|
|
{
|
|
if (setAccessMode(MASTERCONTROLLEDMODE) != BUS_OK)
|
|
{
|
|
ret = TLV493D_BUS_ERROR;
|
|
}
|
|
delay(getMeasurementDelay());
|
|
powerdown = 1;
|
|
}
|
|
if(ret == TLV493D_NO_ERROR)
|
|
{
|
|
#ifdef TLV493D_ACCELERATE_READOUT
|
|
// just read the most important results in FASTMODE, if this behaviour is desired
|
|
if(mMode == FASTMODE)
|
|
{
|
|
if (readOut(&mInterface, TLV493D_FAST_READOUT) != BUS_OK)
|
|
{
|
|
ret = TLV493D_BUS_ERROR;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (readOut(&mInterface, TLV493D_MEASUREMENT_READOUT) != BUS_OK)
|
|
{
|
|
ret = TLV493D_BUS_ERROR;
|
|
}
|
|
}
|
|
#else
|
|
if (readOut(&mInterface, TLV493D_MEASUREMENT_READOUT) != BUS_OK)
|
|
{
|
|
ret = TLV493D_BUS_ERROR;
|
|
}
|
|
#endif
|
|
if (ret == TLV493D_NO_ERROR)
|
|
{
|
|
// construct results from registers
|
|
mXdata = concatResults(getRegBits(tlv493d::R_BX1), getRegBits(tlv493d::R_BX2), true);
|
|
mYdata = concatResults(getRegBits(tlv493d::R_BY1), getRegBits(tlv493d::R_BY2), true);
|
|
mZdata = concatResults(getRegBits(tlv493d::R_BZ1), getRegBits(tlv493d::R_BZ2), true);
|
|
mTempdata = concatResults(getRegBits(tlv493d::R_TEMP1), getRegBits(tlv493d::R_TEMP2), false);
|
|
// switch sensor back to POWERDOWNMODE, if it was in POWERDOWNMODE before
|
|
if(powerdown)
|
|
{
|
|
if (setAccessMode(POWERDOWNMODE) != BUS_OK)
|
|
{
|
|
ret = TLV493D_BUS_ERROR;
|
|
}
|
|
}
|
|
if (ret == TLV493D_NO_ERROR)
|
|
{
|
|
// if the return value is 0, all results are from the same frame
|
|
// otherwise some results may be outdated
|
|
if(getRegBits(tlv493d::R_CHANNEL) != 0)
|
|
{
|
|
ret = TLV493D_FRAME_ERROR;
|
|
}
|
|
// Todo: removed due to a lot of frame errors
|
|
// // if received frame count does not match expected one (frame count from 0 to 3)
|
|
// else if( getRegBits(tlv493d::R_FRAMECOUNTER) != (mExpectedFrameCount % 4) )
|
|
// {
|
|
// ret = TLV493D_FRAME_ERROR;
|
|
// }
|
|
}
|
|
}
|
|
}
|
|
mExpectedFrameCount = getRegBits(tlv493d::R_FRAMECOUNTER) + 1;
|
|
return ret;
|
|
}
|
|
|
|
// SBEZEK
|
|
uint8_t Tlv493d::getExpectedFrameCount(void) {
|
|
return mExpectedFrameCount;
|
|
}
|
|
|
|
float Tlv493d::getX(void)
|
|
{
|
|
return static_cast<float>(mXdata) * TLV493D_B_MULT;
|
|
}
|
|
|
|
|
|
float Tlv493d::getY(void)
|
|
{
|
|
return static_cast<float>(mYdata) * TLV493D_B_MULT;
|
|
}
|
|
|
|
|
|
float Tlv493d::getZ(void)
|
|
{
|
|
return static_cast<float>(mZdata) * TLV493D_B_MULT;
|
|
}
|
|
|
|
|
|
float Tlv493d::getTemp(void)
|
|
{
|
|
return static_cast<float>(mTempdata-TLV493D_TEMP_OFFSET) * TLV493D_TEMP_MULT;
|
|
}
|
|
|
|
|
|
float Tlv493d::getAmount(void)
|
|
{
|
|
// 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 Tlv493d::getAzimuth(void)
|
|
{
|
|
// arctan(y/x)
|
|
return atan2(static_cast<float>(mYdata), static_cast<float>(mXdata));
|
|
}
|
|
|
|
|
|
float Tlv493d::getPolar(void)
|
|
{
|
|
// 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)));
|
|
}
|
|
|
|
|
|
/* internal function called by begin()
|
|
* The sensor has a special reset sequence which allows to change its i2c address by setting SDA to high or low during a reset.
|
|
* As some i2c peripherals may not cope with this, the simplest way is to use for this very few bytes bitbanging on the SCL/SDA lines.
|
|
* Furthermore, as the uC may be stopped during a i2c transmission, a special recovery sequence allows to bring the bus back to
|
|
* an operating state.
|
|
*/
|
|
void Tlv493d::resetSensor(uint8_t adr) // Recovery & Reset - this can be handled by any uC as it uses bitbanging
|
|
{
|
|
mInterface.bus->beginTransmission(0x00);
|
|
|
|
if (adr == TLV493D_ADDRESS1) {
|
|
// if the sensor shall be initialized with i2c address 0x1F
|
|
mInterface.bus->write(0xFF);
|
|
} else {
|
|
// if the sensor shall be initialized with address 0x5E
|
|
mInterface.bus->write((uint8_t)0x00);
|
|
}
|
|
|
|
mInterface.bus->endTransmission(true);
|
|
}
|
|
|
|
void Tlv493d::setRegBits(uint8_t regMaskIndex, uint8_t data)
|
|
{
|
|
if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
|
|
{
|
|
tlv493d::setToRegs(&(tlv493d::regMasks[regMaskIndex]), mInterface.regWriteData, data);
|
|
}
|
|
}
|
|
|
|
uint8_t Tlv493d::getRegBits(uint8_t regMaskIndex)
|
|
{
|
|
if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
|
|
{
|
|
const tlv493d::RegMask_t *mask = &(tlv493d::regMasks[regMaskIndex]);
|
|
if(mask->rw == REGMASK_READ)
|
|
{
|
|
return tlv493d::getFromRegs(mask, mInterface.regReadData);
|
|
}
|
|
else
|
|
{
|
|
return tlv493d::getFromRegs(mask, mInterface.regWriteData);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void Tlv493d::calcParity(void)
|
|
{
|
|
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(tlv493d::W_PARITY, 1);
|
|
// combine array to one byte first
|
|
for(i = 0; i < TLV493D_BUSIF_WRITESIZE; i++)
|
|
{
|
|
y ^= mInterface.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(tlv493d::W_PARITY, y&0x01);
|
|
}
|
|
|
|
|
|
int16_t Tlv493d::concatResults(uint8_t upperByte, uint8_t lowerByte, bool upperFull)
|
|
{
|
|
int16_t value=0x0000; //16-bit signed integer for 12-bit values of sensor
|
|
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;
|
|
}
|
|
|