Firmware updates - sensors, calibration, View support, etc (#9)

- 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
This commit is contained in:
Scott Bezek 2022-03-10 19:05:49 -08:00 committed by GitHub
parent 9e2725f850
commit b47fcf7da4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
26 changed files with 2101 additions and 110 deletions

1
firmware/lib/tlv/.piopm Normal file
View File

@ -0,0 +1 @@
{"type": "library", "name": "TLV493D-Magnetic-Sensor", "version": "1.0.3", "spec": {"owner": "infineon", "id": 957, "name": "TLV493D-Magnetic-Sensor", "requirements": null, "url": null}}

View File

@ -0,0 +1,35 @@
# License Summary for Repository
```
Important Notice:
Changes, suggestions and commits in this repository may only be done putting them
under the same license of the respective file.
All rights of the respective copyright holders shall be reserved.
Brands and product names are trademarks of their respective owners.
Referred and linked files/pages are out-of-scope of this repository and underly
their respective licenses.
```
## License
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.

View File

@ -0,0 +1,72 @@
# TLV493D-A1B6-3DMagnetic-Sensor
[![Build Status](https://travis-ci.org/Infineon/TLV493D-A1B6-3DMagnetic-Sensor.svg?branch=master)](https://travis-ci.org/Infineon/TLV493D-A1B6-3DMagnetic-Sensor)
<img src="https://github.com/Infineon/Assets/blob/master/Pictures/3D%20Magnetic%20Sensor%202Go.jpg" width=250> <img src="https://github.com/Infineon/Assets/blob/master/Pictures/TLV493D-Sense-Shield2Go_Top_plain.jpg_2045671804.jpg?raw=true" width=300>
Library of Infineon's [TLV493D-A1B6 3D magnetic sensor](https://www.infineon.com/cms/en/product/sensor/magnetic-sensors/magnetic-position-sensors/3d-magnetics/tlv493d-a1b6/) for Arduino.
## Summary
The 3D magnetic sensor TLV493D-A1B6 offers accurate three-dimensional sensing with extremely low power consumption in a small 6-pin package. With its magnetic field detection in x, y, and z-direction the sensor reliably measures three-dimensional, linear and rotation movements. Applications include joysticks, control elements (white goods, multifunction knops), or electric meters (anti tampering) and any other application that requires accurate angular measurements or low power consumptions.
The integrated temperature sensor can furthermore be used for plausibility checks.
Key features are 3D magnetic sensing with a very low power consumption during operations. The sensor has a digital output via 2-wire based standard I2C interface up to 1 MBit/sec and 12 bit data resolution for each measurement direction (Bx, By and Bz linear field measurement up to +-130mT).
## Key Features and Benefits
* Integrated temperature sensing
* Low current consumption of 0.007 µA in power down mode and 10 µA in ultra low power mode
* 2.7 to 3.5 V operating supply voltage
* Digital output via 2-wire standard I2C interface
* Bx, By and Bz linear field measurement up to ±130 mT
* 12-bit data resolution for each measurement direction
* Resolution 98 µT/LSB
* Operating temperature range from -40 °C to 125 °C
* [GUI](https://www.infineon.com/dgdl/Infineon-Software-for-3D-Magnetic-Sensor-2Go%20incl.%20out-of-shaft_05_06-SW-v05_06-EN.zip?fileId=5546d4626102d35a01614626f9644e4e) for free download as well as integration in Arduino IDE with this repository
## Installation
### Integration of Library
Please download this repository from GitHub by clicking on the following field in the [releases](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/releases) of this repository.
![Download Library](https://raw.githubusercontent.com/infineon/assets/master/Pictures/Releases_Generic.jpg)
To install the 3D magnetic sensor 2GO library in the Arduino IDE, please go now to **Sketch** > **Include Library** > **Add .ZIP Library...** in the Arduino IDE and navigate to the downloaded .ZIP file of this repository. The library will be installed in your Arduino sketch folder in libraries and you can select as well as include this one to your project under **Sketch** > **Include Library** > **TLV493D-A1B6**.
![Install Library](https://raw.githubusercontent.com/infineon/assets/master/Pictures/Library_Install_ZIP.png)
## Usage
Please see the example sketches in the `/examples` directory in this library to learn more about the usage of the library.
Currently, there exist two separate evaluation boards:
* [TLV493D-A1B6 MS2GO](https://www.infineon.com/cms/de/product/evaluation-boards/tlv493d-a1b6-ms2go/)
* [TLV493D-A1B6 3DMagnetic Shield2Go](https://www.infineon.com/cms/en/product/evaluation-boards/s2go_3d-sense_tlv493d/)
### Usage with TLV493D-A1B6 MS2GO
The 3D Magnetic Sensor 2GO is an evaluation board equipped with the magnetic sensor [TLV493D-A1B6](https://www.infineon.com/cms/en/product/evaluation-boards/tlv493d-a1b6-ms2go/) for three dimensional measurement combined with an ARM® Cortex™-M0 CPU. The 3D Magnetic Sensor 2GO has a complete set of on-board devices, including an on-board debugger. A PDF summarizing the features and layout of the 3D magnetic sensor 2GO board is stored on the Infineon homepage [here](https://www.infineon.com/dgdl/Infineon-3D-Magnetic-Sensor_EvalKit_UM-UM-v01_01-EN.pdf?fileId=5546d462525dbac40152ac4ca1d318c2).
Please note that base of the Sensors 2GO is the XMC 2Go from Infineon. Therefore, please install (if not already done) also the [XMC-for-Arduino](https://github.com/Infineon/XMC-for-Arduino) implementation and choose afterwards **XMC1100 XMC2Go** from the **Tools**>**Board** menu in the Arduino IDE if working with this evaluation board.
### TLV493D-A1B6 3DSense Shield2Go
The TLV493D-A1B6 3DMagnetic Shield2Go is a standalone break out board with Infineon's Shield2Go formfactor and pin out. You can connect it easily to any microcontroller of your choice which is Arduino compatible and has 3.3V logic level (please note that the Arduino UNO has 5V logic level and cannot be used without level shifting).
* [Link](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/wiki) to the wiki with more information
However, every Shield2Go is directly compatible with Infineon's XMC2Go and the recommended quick start is to use an XMC2Go for evaluation. Therefore, please install (if not already done) also the [XMC-for-Arduino](https://github.com/Infineon/XMC-for-Arduino) implementation and choose afterwards **XMC1100 XMC2Go** from the **Tools**>**Board** menu in the Arduino IDE if working with this evaluation board. To use it, please plug the TLV493D-A1B6 3DMagnetic Shield2Go onto the XMC2Go as shown below.
<img src="https://github.com/Infineon/Assets/blob/master/Pictures/TLV493D-A1B6_S2Go_w_XMC2Go.png" width=250>
## Processing
This library supports the open-source software [Processing](https://processing.org/) for creating GUIs. It allows you to connect your evaluation board to a PC over serial communication and visualisation of the embedded system. Find out more on the Arduino homepage [here](http://playground.arduino.cc/Interfacing/Processing). The respective files are stored in the /processing folder of this repository.
## Printables
The TLx493D 3D magnetic sensor family has additional tools which can be directly mounted on top of the evaluation boards. The 3D print data of the [joystick](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO), the [rotate knob](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO) and the [linear slider](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO) can be found in the folder `printables`.
<img src="https://www.infineon.com/export/sites/default/media/products/Sensors/joystick.jpg_708092179.jpg" width=250>
## Board Information, Datasheet and Additional Information
The datasheet for the TLV493D-A1B6 can be found here [TLV493D-A1B6 Datasheet](https://www.infineon.com/dgdl/Infineon-TLV493D-A1B6-DS-v01_00-EN.pdf?fileId=5546d462525dbac40152a6b85c760e80) while respective application notes are located here [Application Notes](https://www.infineon.com/dgdl/Infineon-TLV493D-A1B6_3DMagnetic-UM-v01_03-EN.pdf?fileId=5546d46261d5e6820161e75721903ddd).
Please check the [wiki](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/wiki) with more information for the TLV493D-A1B6 3DSense Shield2Go as well.

View File

@ -0,0 +1,17 @@
{
"name": "TLV493D-Magnetic-Sensor",
"keywords": "3D, magnetic, sensor2go",
"description": "This library provides an Interface for Infineon's TLV493D Magnetic Sensor",
"repository":
{
"type": "git",
"url": "https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor.git",
"branch": "master"
},
"version":"1.0.3",
"frameworks": "arduino",
"platforms": [
"infineonxmc",
"atmelavr"
]
}

View File

@ -0,0 +1,10 @@
name=TLV493D-A1B6
version=1.0.3
author=Infineon Technologies
maintainer=Infineon Technologies <www.infineon.com>
sentence=This library provides an Interface for Infineon's TLV493D-A1B6 3D Magnetic Sensor.
paragraph=With this Sensor, you can detect the position of a Joystick etc. without any additional mechanical components. You just have to provide a magnet that is fixed to a movable component.
category=Sensors
url=http://www.infineon.com/cms/de/product/sensor/magnetic-position-sensor/3d-magnetic-sensor/TLV493D-A1B6/productType.html?productType=5546d462525dbac401529cebc74f07b7
architectures=*

View File

@ -0,0 +1,369 @@
/**
* 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;
}

View File

@ -0,0 +1,153 @@
/**
* Tlv493d.h - 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.
*/
#ifndef TLV493D_H_INCLUDED
#define TLV493D_H_INCLUDED
#include <Arduino.h>
#include <Wire.h>
#include "./util/BusInterface.h"
#include "./util/Tlv493d_conf.h"
typedef enum Tlv493d_Address
{
TLV493D_ADDRESS1 = 0x5E,
TLV493D_ADDRESS2 = 0x1F
}Tlv493d_Address_t;
typedef enum Tlv493d_Error
{
TLV493D_NO_ERROR = 0,
TLV493D_BUS_ERROR = 1,
TLV493D_FRAME_ERROR = 2
}Tlv493d_Error_t;
/*
* TLV493D_ACCELERATE_READOUT lets the controller just read out the first 3 bytes when in fast mode.
* This makes the readout faster (half of usual transfer duration), but there is no way to get
* temperature, current channel or high precision (only 8 instead of 12 bits for x, y, z)
* It is necessary for slow I2C busses to read the last result before the new measurement is completed.
* It only takes effect in FASTMODE, not in other modes.
*
* Feel free to undefine this and increase your I2C bus speed if you need to.
*/
// SBEZEK
// #define TLV493D_ACCELERATE_READOUT
class Tlv493d
{
public:
Tlv493d(void);
~Tlv493d(void);
void begin(void);
void begin(TwoWire &bus);
void begin(TwoWire &bus, Tlv493d_Address_t slaveAddress, bool reset);
void end(void);
// sensor configuration
/* sets the data access mode for TLE493D
* Tle493d is initially in POWERDOWNMODE
* use POWERDOWNMODE for rare and infrequent measurements
* Tle493d will automatically switch to MASTERCONTROLLEDMODE for one measurement if on a readout
* measurements are quite slow in this mode. The power consumption is very low between measurements.
* use MASTERCONTROLLEDMODE for low measurement frequencies where results do not have to be up-to-date
* In this mode a new measurement starts directly after the last result has been read out.
* use LOWPOWERMODE and ULTRALOWPOWERMODE for continuous measurements
* each readout returns the latest measurement results
* use FASTMODE for for continuous measurements on high frequencies
* measurement time might be higher than the time necessary for I2C-readouts in this mode.
* Note: Thus, this mode requires a non-standard 1MHz I2C clock to be used to read the data fast enough.
*/
enum AccessMode_e
{
POWERDOWNMODE = 0,
FASTMODE,
LOWPOWERMODE,
ULTRALOWPOWERMODE,
MASTERCONTROLLEDMODE,
};
bool setAccessMode(AccessMode_e mode);
// interrupt is disabled by default
// it is recommended for FASTMODE, LOWPOWERMODE and ULTRALOWPOWERMODE
// the interrupt is indicated with a short(1.5 us) low pulse on SCL
// you need to capture and react(read the new results) to it by yourself
void enableInterrupt(void);
void disableInterrupt(void);
// temperature measurement is enabled by default
// it can be disabled to reduce power consumption
void enableTemp(void);
void disableTemp(void);
// returns the recommended time between two readouts for the sensor's current configuration
uint16_t getMeasurementDelay(void);
// read measurement results from sensor
Tlv493d_Error_t updateData(void);
// fieldvector in Cartesian coordinates
float getX(void);
float getY(void);
float getZ(void);
// fieldvector in spherical coordinates
float getAmount(void);
float getAzimuth(void);
float getPolar(void);
// temperature
float getTemp(void);
// SBEZEK
uint8_t getExpectedFrameCount(void);
private:
tlv493d::BusInterface_t mInterface;
AccessMode_e mMode;
int16_t mXdata;
int16_t mYdata;
int16_t mZdata;
int16_t mTempdata;
uint8_t mExpectedFrameCount;
void resetSensor(uint8_t adr);
void setRegBits(uint8_t regMaskIndex, uint8_t data);
uint8_t getRegBits(uint8_t regMaskIndex);
void calcParity(void);
int16_t concatResults(uint8_t upperByte, uint8_t lowerByte, bool upperFull);
};
#endif /* TLV493D_H_INCLUDED */

View File

@ -0,0 +1,97 @@
/**
* BusInterface.cpp - Part of the 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 "BusInterface2.h"
void tlv493d::initInterface(BusInterface_t *interface, TwoWire *bus, uint8_t adress)
{
uint8_t i;
interface->bus = bus;
interface->adress = adress;
for(i = 0; i < TLV493D_BUSIF_READSIZE; i++) {
interface->regReadData[i] = 0x00;;
}
for(i = 0; i < TLV493D_BUSIF_WRITESIZE; i++) {
interface->regWriteData[i] = 0x00;;
}
}
bool tlv493d::readOut(BusInterface_t *interface)
{
return readOut(interface, TLV493D_BUSIF_READSIZE);
}
bool tlv493d::readOut(BusInterface_t *interface, uint8_t count)
{
bool ret = BUS_ERROR;
int i;
if(count > TLV493D_BUSIF_READSIZE)
{
count = TLV493D_BUSIF_READSIZE;
}
uint8_t received_bytes = interface->bus->requestFrom(interface->adress,count);
if (received_bytes == count)
{
for(i = 0; i < count; i++)
{
interface->regReadData[i] = interface->bus->read();
}
ret = BUS_OK;
}
return ret;
}
bool tlv493d::writeOut(BusInterface_t *interface)
{
return writeOut(interface, TLV493D_BUSIF_WRITESIZE);
}
bool tlv493d::writeOut(BusInterface_t *interface, uint8_t count)
{
bool ret = BUS_ERROR;
int i;
if(count > TLV493D_BUSIF_WRITESIZE)
{
count = TLV493D_BUSIF_WRITESIZE;
}
interface->bus->beginTransmission(interface->adress);
for(i = 0; i < count; i++)
{
interface->bus->write(interface->regWriteData[i]);
}
if (interface->bus->endTransmission() == 0)
{
ret = BUS_OK;
}
return ret;
}

View File

@ -0,0 +1,55 @@
/**
* BusInterface.h - Part of the 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.
*/
#ifndef TLV493D_BUSIF_H_INCLUDED
#define TLV493D_BUSIF_H_INCLUDED
#include <Arduino.h>
#include <Wire.h>
#define TLV493D_BUSIF_READSIZE 10
#define TLV493D_BUSIF_WRITESIZE 4
namespace tlv493d
{
typedef struct
{
TwoWire *bus;
uint8_t adress;
uint8_t regReadData[TLV493D_BUSIF_READSIZE];
uint8_t regWriteData[TLV493D_BUSIF_WRITESIZE];
} BusInterface_t;
}
#endif

View File

@ -0,0 +1,52 @@
/**
* BusInterface2.h - Part of the 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.
*/
#ifndef TLV493D_BUSIF_2_H_INCLUDED
#define TLV493D_BUSIF_2_H_INCLUDED
#include "BusInterface.h"
#define BUS_ERROR 1
#define BUS_OK 0
namespace tlv493d
{
void initInterface(BusInterface_t *interface, TwoWire *bus, uint8_t adress);
bool readOut(BusInterface_t *interface);
bool readOut(BusInterface_t *interface, uint8_t count);
bool writeOut(BusInterface_t *interface);
bool writeOut(BusInterface_t *interface, uint8_t count);
}
#endif

View File

@ -0,0 +1,54 @@
/**
* RegMask.cpp - Part of the 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 "RegMask.h"
uint8_t tlv493d::getFromRegs(const RegMask_t *mask, uint8_t *regData)
{
return (regData[mask->byteAdress] & mask->bitMask) >> mask->shift;
}
uint8_t tlv493d::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;
}
return 0;
}

View File

@ -0,0 +1,57 @@
/**
* RegMask.h - Part of the 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.
*/
#ifndef TLV493D_REGMASK_H_INCLUDED
#define TLV493D_REGMASK_H_INCLUDED
#include <Arduino.h>
#define REGMASK_READ 0
#define REGMASK_WRITE 1
namespace tlv493d
{
typedef struct
{
uint8_t rw;
uint8_t byteAdress;
uint8_t bitMask;
uint8_t shift;
} RegMask_t;
uint8_t getFromRegs(const RegMask_t *mask, uint8_t *regData);
uint8_t setToRegs(const RegMask_t *mask, uint8_t *regData, uint8_t toWrite);
}
#endif

View File

@ -0,0 +1,134 @@
/**
* Tlv493d_conf.h - Part of the 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.
*/
#ifndef TLV493D_CONF_H_INCLUDED
#define TLV493D_CONF_H_INCLUDED
#include "RegMask.h"
#include "Tlv493d.h"
#define TLV493D_DEFAULTMODE POWERDOWNMODE
#define TLV493D_STARTUPDELAY 40
#define TLV493D_RESETDELAY 60
#define TLV493D_NUM_OF_REGMASKS 25
#define TLV493D_NUM_OF_ACCMODES 5
#define TLV493D_MEASUREMENT_READOUT 7
#define TLV493D_FAST_READOUT 3
#define TLV493D_B_MULT 0.098
#define TLV493D_TEMP_MULT 1.1
#define TLV493D_TEMP_OFFSET 315
namespace tlv493d
{
typedef struct
{
uint8_t fast;
uint8_t lp;
uint8_t lpPeriod;
uint16_t measurementTime;
} AccessMode_t;
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
};
const RegMask_t regMasks[] = {
{ 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
};
const AccessMode_t accModes[] = {
{ 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
};
}
#endif

View File

@ -8,9 +8,8 @@
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:tdisplay]
platform = espressif32
board = esp32doit-devkit-v1
[base_config]
platform = espressif32@3.4
framework = arduino
monitor_speed = 115200
monitor_flags =
@ -18,14 +17,41 @@ monitor_flags =
--echo
--filter=esp32_exception_decoder
lib_deps =
bodmer/TFT_eSPI@2.4.25
askuric/Simple FOC @ ^2.2
infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
bxparks/AceButton @ ^1.9.1
askuric/Simple FOC @ 2.2.0
infineon/TLV493D-Magnetic-Sensor @ 1.0.3
bxparks/AceButton @ 1.9.1
build_flags =
-DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
[env:proto]
extends = base_config
board = esp32doit-devkit-v1
lib_deps =
${base_config.lib_deps}
bodmer/TFT_eSPI@2.4.25
build_flags =
${base_config.build_flags}
-DSK_DISPLAY=1
-DPIN_UH=27
-DPIN_UL=26
-DPIN_VH=25
-DPIN_VL=33
-DPIN_WH=32
-DPIN_WL=13
-DPIN_BUTTON_NEXT=36
-DPIN_BUTTON_PREV=-1
-DPIN_SDA=-1
-DPIN_SCL=-1
-DSENSOR_MT6701=1
-DDESCRIPTION_FONT=Roboto_Thin_24
-DDESCRIPTION_Y_OFFSET=20
-DVALUE_OFFSET=30
-DDRAW_ARC=0
-DUSER_SETUP_LOADED=1
-DGC9A01_DRIVER=1
-DCGRAM_OFFSET=1
@ -41,3 +67,152 @@ build_flags =
-DLOAD_GLCD=1
-DLOAD_GFXFF=1
-DSPI_FREQUENCY=40000000
[env:view]
extends = base_config
; platform = https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream
; platform_packages =
; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32#master
board = esp32doit-devkit-v1
lib_deps =
; askuric/Simple FOC @ 2.2.1
; bxparks/AceButton @ 1.9.1
${base_config.lib_deps}
bodmer/TFT_eSPI@2.4.25
fastled/FastLED @ 3.5.0
bogde/HX711 @ 0.7.5
adafruit/Adafruit VEML7700 Library @ 1.1.1
build_flags =
${base_config.build_flags}
-DSK_DISPLAY=1
-DSK_LEDS=1
-DNUM_LEDS=8
-DSENSOR_MT6701=1
-DSK_STRAIN=1
-DSK_INVERT_ROTATION=1
-DSK_ALS=1
-DPIN_UH=26
-DPIN_UL=25
-DPIN_VH=27
-DPIN_VL=32
-DPIN_WH=12
-DPIN_WL=33
-DPIN_BUTTON_NEXT=-1
-DPIN_BUTTON_PREV=-1
-DPIN_SDA=15
-DPIN_SCL=8
-DPIN_MT_DATA=37
-DPIN_MT_CLOCK=13
-DPIN_MT_CSN=14
-DPIN_LED_DATA=7
-DPIN_LCD_BACKLIGHT=19
-DLEDC_CHANNEL_LCD_BACKLIGHT=0
-DDESCRIPTION_FONT=Roboto_Thin_24
-DDESCRIPTION_Y_OFFSET=20
-DVALUE_OFFSET=30
-DDRAW_ARC=0
-DUSER_SETUP_LOADED=1
-DGC9A01_DRIVER=1
-DCGRAM_OFFSET=1
-DTFT_WIDTH=240
-DTFT_HEIGHT=240
-DTFT_MISO=-1
-DTFT_MOSI=5
-DTFT_SCLK=20
-DTFT_CS=21
-DTFT_DC=22
-DTFT_RST=4
-DTFT_BL=-1
-DLOAD_GLCD=1
-DLOAD_GFXFF=1
-DSPI_FREQUENCY=40000000
; Reduce loop task stack size (only works on newer IDF Arduino core)
; -DARDUINO_LOOP_STACK_SIZE=2048
; Modify the default unusable pin mask to allow GPIO 7 (allowed to use on ESP32-PICO-V3-02)
; Unusable bits: 6, 8, 9, 10, 20
; (0ULL | _FL_BIT(6) | _FL_BIT(8) | _FL_BIT(9) | _FL_BIT(10) | _FL_BIT(20))
-DFASTLED_UNUSABLE_PIN_MASK=0x100740LL
; 0~39 except from 24, 28~31 are valid
; (0xFFFFFFFFFFULL & ~(0ULL | _FL_BIT(24) | _FL_BIT(28) | _FL_BIT(29) | _FL_BIT(30) | _FL_BIT(31)))
-DSOC_GPIO_VALID_GPIO_MASK=0xFF0EFFFFFF
; GPIO >= 34 are input only
; (SOC_GPIO_VALID_GPIO_MASK & ~(0ULL | _FL_BIT(34) | _FL_BIT(35) | _FL_BIT(36) | _FL_BIT(37) | _FL_BIT(38) | _FL_BIT(39)))
-DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
[env:handheld]
extends = base_config
board = tinypico
build_flags =
${base_config.build_flags}
-DSK_DISPLAY=0
-DPIN_UH=25
-DPIN_UL=26
-DPIN_VH=27
-DPIN_VL=15
-DPIN_WH=14
-DPIN_WL=4
-DPIN_BUTTON_NEXT=23
-DPIN_BUTTON_PREV=-1
-DPIN_SDA=33
-DPIN_SCL=32
-DSENSOR_TLV=1
[env:handheld_tdisplay]
extends = base_config
board = esp32doit-devkit-v1
lib_deps =
${base_config.lib_deps}
bodmer/TFT_eSPI@2.4.25
build_flags =
${base_config.build_flags}
-DSK_DISPLAY=1
-DSK_STRAIN=0
-DSK_LEDS=0
-DPIN_UH=17
-DPIN_UL=2
-DPIN_VH=13
-DPIN_VL=32
-DPIN_WH=33
-DPIN_WL=25
-DPIN_BUTTON_NEXT=35
-DPIN_BUTTON_PREV=0
-DPIN_SDA=-1
-DPIN_SCL=-1
-DSENSOR_MT6701=1
-DPIN_MT_DATA=21
-DPIN_MT_CLOCK=22
-DPIN_MT_CSN=12
-DDESCRIPTION_FONT=FreeSans9pt7b
-DDESCRIPTION_Y_OFFSET=80
-DVALUE_OFFSET=0
-DDRAW_ARC=1
-DUSER_SETUP_LOADED=1
-DST7789_DRIVER=1
-DCGRAM_OFFSET=1
-DTFT_WIDTH=135
-DTFT_HEIGHT=240
-DTFT_MISO=-1
-DTFT_MOSI=19
-DTFT_SCLK=18
-DTFT_CS=5
-DTFT_DC=16
-DTFT_RST=23
-DTFT_BL=4
-DLOAD_GLCD=1
-DLOAD_GFXFF=1
-DSPI_FREQUENCY=40000000

View File

@ -1,18 +1,20 @@
#if SK_DISPLAY
#include "display_task.h"
#include "semaphore_guard.h"
#include "font/roboto_light_60.h"
DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} {
semaphore_ = xSemaphoreCreateMutex();
assert(semaphore_ != NULL);
xSemaphoreGive(semaphore_);
DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 4048, 1, task_core} {
knob_state_queue_ = xQueueCreate(1, sizeof(KnobState));
assert(knob_state_queue_ != NULL);
mutex_ = xSemaphoreCreateMutex();
assert(mutex_ != NULL);
}
DisplayTask::~DisplayTask() {
if (semaphore_ != NULL) {
vSemaphoreDelete(semaphore_);
}
vQueueDelete(knob_state_queue_);
vSemaphoreDelete(mutex_);
}
static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
@ -73,16 +75,23 @@ static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_
}
void DisplayTask::run() {
delay(100);
tft_.begin();
tft_.invertDisplay(1);
tft_.setRotation(0);
tft_.fillScreen(TFT_PURPLE);
tft_.fillScreen(TFT_DARKGREEN);
ledcSetup(LEDC_CHANNEL_LCD_BACKLIGHT, 5000, 16);
ledcAttachPin(PIN_LCD_BACKLIGHT, LEDC_CHANNEL_LCD_BACKLIGHT);
ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, UINT16_MAX);
spr_.setColorDepth(16);
if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) {
Serial.println("ERROR: sprite allocation failed!");
tft_.fillScreen(TFT_RED);
} else {
Serial.println("Sprite created!");
tft_.fillScreen(TFT_PURPLE);
}
spr_.setTextColor(0xFFFF, TFT_BLACK);
@ -100,9 +109,8 @@ void DisplayTask::run() {
spr_.setTextDatum(CC_DATUM);
spr_.setTextColor(TFT_WHITE);
while(1) {
{
SemaphoreGuard lock(semaphore_);
state = state_;
if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
continue;
}
spr_.fillSprite(TFT_BLACK);
@ -112,9 +120,9 @@ void DisplayTask::run() {
}
spr_.setFreeFont(&Roboto_Light_60);
spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - 30, 1);
spr_.setFreeFont(&Roboto_Thin_24);
int32_t line_y = TFT_HEIGHT / 2 + 20;
spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - VALUE_OFFSET, 1);
spr_.setFreeFont(&DESCRIPTION_FONT);
int32_t line_y = TFT_HEIGHT / 2 + DESCRIPTION_Y_OFFSET;
char* start = state.config.descriptor;
char* end = start + strlen(state.config.descriptor);
while (start < end) {
@ -139,6 +147,9 @@ void DisplayTask::run() {
spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(left_bound), TFT_HEIGHT/2 - RADIUS * sinf(left_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(left_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(left_bound), TFT_WHITE);
spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(right_bound), TFT_HEIGHT/2 - RADIUS * sinf(right_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(right_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(right_bound), TFT_WHITE);
}
if (DRAW_ARC) {
spr_.drawCircle(TFT_WIDTH/2, TFT_HEIGHT/2, RADIUS, TFT_DARKGREY);
}
float adjusted_sub_position = state.sub_position_unit * state.config.position_width_radians;
if (state.config.num_positions > 0) {
@ -171,11 +182,22 @@ void DisplayTask::run() {
}
spr_.pushSprite(0, 0);
{
SemaphoreGuard lock(mutex_);
ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, brightness_);
}
delay(2);
}
}
void DisplayTask::setData(KnobState state) {
SemaphoreGuard lock(semaphore_);
state_ = state;
QueueHandle_t DisplayTask::getKnobStateQueue() {
return knob_state_queue_;
}
void DisplayTask::setBrightness(uint16_t brightness) {
SemaphoreGuard lock(mutex_);
brightness_ = brightness;
}
#endif

View File

@ -1,5 +1,7 @@
#pragma once
#if SK_DISPLAY
#include <Arduino.h>
#include <TFT_eSPI.h>
@ -13,7 +15,9 @@ class DisplayTask : public Task<DisplayTask> {
DisplayTask(const uint8_t task_core);
~DisplayTask();
void setData(KnobState state);
QueueHandle_t getKnobStateQueue();
void setBrightness(uint16_t brightness);
protected:
void run();
@ -24,7 +28,17 @@ class DisplayTask : public Task<DisplayTask> {
/** Full-size sprite used as a framebuffer */
TFT_eSprite spr_ = TFT_eSprite(&tft_);
SemaphoreHandle_t semaphore_;
QueueHandle_t knob_state_queue_;
KnobState state_;
SemaphoreHandle_t mutex_;
uint16_t brightness_;
};
#else
class DisplayTask {};
#endif

View File

@ -1,10 +1,36 @@
#include <AceButton.h>
#if SK_LEDS
#include <FastLED.h>
#endif
#if SK_STRAIN
#include <HX711.h>
#endif
#if SK_ALS
#include <Adafruit_VEML7700.h>
#endif
#include "interface_task.h"
#include "util.h"
using namespace ace_button;
#define COUNT_OF(A) (sizeof(A) / sizeof(A[0]))
#if SK_LEDS
CRGB leds[NUM_LEDS];
#endif
#if SK_STRAIN
HX711 scale;
#endif
#if SK_ALS
Adafruit_VEML7700 veml = Adafruit_VEML7700();
#endif
static KnobConfig configs[] = {
// int32_t num_positions;
// int32_t position;
@ -81,7 +107,7 @@ static KnobConfig configs[] = {
32,
0,
8.225806452 * PI / 180,
1,
2,
1,
1.1,
"Coarse values\nStrong detents",
@ -97,25 +123,146 @@ static KnobConfig configs[] = {
},
};
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : Task{"Interface", 2048, 1, task_core}, motor_task_(motor_task) {
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : Task("Interface", 4048, 1, task_core), motor_task_(motor_task), display_task_(display_task) {
#if SK_DISPLAY
assert(display_task != nullptr);
#endif
}
InterfaceTask::~InterfaceTask() {}
void InterfaceTask::run() {
AceButton button(36);
pinMode(36, INPUT);
button.getButtonConfig()->setIEventHandler(this);
#if PIN_BUTTON_NEXT >= 34
pinMode(PIN_BUTTON_NEXT, INPUT);
#else
pinMode(PIN_BUTTON_NEXT, INPUT_PULLUP);
#endif
AceButton button_next((uint8_t) PIN_BUTTON_NEXT);
button_next.getButtonConfig()->setIEventHandler(this);
#if PIN_BUTTON_PREV > -1
#if PIN_BUTTON_PREV >= 34
pinMode(PIN_BUTTON_PREV, INPUT);
#else
pinMode(PIN_BUTTON_PREV, INPUT_PULLUP);
#endif
AceButton button_prev((uint8_t) PIN_BUTTON_PREV);
button_prev.getButtonConfig()->setIEventHandler(this);
#endif
#if SK_LEDS
FastLED.addLeds<SK6812, PIN_LED_DATA, GRB>(leds, NUM_LEDS);
#endif
#if PIN_SDA >= 0 && PIN_SCL >= 0
Wire.begin(PIN_SDA, PIN_SCL);
Wire.setClock(400000);
#endif
#if SK_STRAIN
scale.begin(38, 2);
#endif
#if SK_ALS
if (veml.begin()) {
veml.setGain(VEML7700_GAIN_2);
veml.setIntegrationTime(VEML7700_IT_400MS);
} else {
Serial.println("ALS sensor not found!");
}
#endif
motor_task_.setConfig(configs[0]);
// How far button is pressed, in range [0, 1]
float press_value_unit = 0;
// Interface loop:
while (1) {
button.check();
button_next.check();
#if PIN_BUTTON_PREV > -1
button_prev.check();
#endif
if (Serial.available()) {
int v = Serial.read();
if (v == ' ') {
nextConfig();
changeConfig(true);
}
}
#if SK_ALS
const float LUX_ALPHA = 0.005;
static float lux_avg;
float lux = veml.readLux();
lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
static uint32_t last_als;
if (millis() - last_als > 1000) {
Serial.print("millilux: "); Serial.println(lux*1000);
last_als = millis();
}
#endif
#if SK_STRAIN
// TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
const int32_t lower = 950000;
const int32_t upper = 1800000;
if (scale.wait_ready_timeout(100)) {
int32_t reading = scale.read();
// Ignore readings that are way out of expected bounds
if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
static uint32_t last_reading_display;
if (millis() - last_reading_display > 1000) {
Serial.print("HX711 reading: ");
Serial.println(reading);
last_reading_display = millis();
}
long value = CLAMP(reading, lower, upper);
press_value_unit = 1. * (value - lower) / (upper - lower);
static bool pressed;
if (!pressed && press_value_unit > 0.75) {
motor_task_.playHaptic(true);
pressed = true;
changeConfig(true);
} else if (pressed && press_value_unit < 0.25) {
motor_task_.playHaptic(false);
pressed = false;
}
}
} else {
Serial.println("HX711 not found.");
#if SK_LEDS
for (uint8_t i = 0; i < NUM_LEDS; i++) {
leds[i] = CRGB::Red;
}
FastLED.show();
#endif
}
#endif
uint16_t brightness = UINT16_MAX;
// TODO: brightness scale factor should be configurable (depends on reflectivity of surface)
#if SK_ALS
brightness = (uint16_t)CLAMP(lux_avg * 13000, (float)1280, (float)UINT16_MAX);
#endif
#if SK_DISPLAY
display_task_->setBrightness(brightness); // TODO: apply gamma correction
#endif
#if SK_LEDS
for (uint8_t i = 0; i < NUM_LEDS; i++) {
leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
// Gamma adjustment
leds[i].r = dim8_video(leds[i].r);
leds[i].g = dim8_video(leds[i].g);
leds[i].b = dim8_video(leds[i].b);
}
FastLED.show();
#endif
delay(10);
}
}
@ -123,15 +270,34 @@ void InterfaceTask::run() {
void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t button_state) {
switch (event_type) {
case AceButton::kEventPressed:
nextConfig();
if (button->getPin() == PIN_BUTTON_NEXT) {
changeConfig(true);
}
#if PIN_BUTTON_PREV > -1
if (button->getPin() == PIN_BUTTON_PREV) {
changeConfig(false);
}
#endif
break;
case AceButton::kEventReleased:
break;
}
}
void InterfaceTask::nextConfig() {
current_config_ = (current_config_ + 1) % COUNT_OF(configs);
Serial.printf("Changing config to %d:\n%s\n", current_config_, configs[current_config_].descriptor);
void InterfaceTask::changeConfig(bool next) {
if (next) {
current_config_ = (current_config_ + 1) % COUNT_OF(configs);
} else {
if (current_config_ == 0) {
current_config_ = COUNT_OF(configs) - 1;
} else {
current_config_ --;
}
}
Serial.print("Changing config to ");
Serial.print(current_config_);
Serial.print(" -- ");
Serial.println(configs[current_config_].descriptor);
motor_task_.setConfig(configs[current_config_]);
}

View File

@ -3,6 +3,7 @@
#include <AceButton.h>
#include <Arduino.h>
#include "display_task.h"
#include "motor_task.h"
#include "task.h"
@ -10,7 +11,7 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
friend class Task<InterfaceTask>; // Allow base Task to invoke protected run()
public:
InterfaceTask(const uint8_t task_core, MotorTask& motor_task);
InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task);
~InterfaceTask();
void handleEvent(ace_button::AceButton* button, uint8_t event_type, uint8_t button_state) override;
@ -20,7 +21,9 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
private:
MotorTask& motor_task_;
DisplayTask* display_task_;
int current_config_ = 0;
void nextConfig();
void changeConfig(bool next);
};

View File

@ -1,31 +1,66 @@
#include <Arduino.h>
#include <FastLED.h>
#include <SimpleFOC.h>
#include <TFT_eSPI.h>
#include "display_task.h"
#include "interface_task.h"
#include "motor_task.h"
#include "tlv_sensor.h"
DisplayTask display_task = DisplayTask(1);
MotorTask motor_task = MotorTask(0, display_task);
InterfaceTask interface_task = InterfaceTask(1, motor_task);
#if SK_DISPLAY
static DisplayTask display_task = DisplayTask(0);
static DisplayTask* display_task_p = &display_task;
#else
static DisplayTask* display_task_p = nullptr;
#endif
static MotorTask motor_task = MotorTask(1);
CRGB leds[1];
InterfaceTask interface_task = InterfaceTask(0, motor_task, display_task_p);
static QueueHandle_t knob_state_debug_queue;
void setup() {
Serial.begin(115200);
motor_task.begin();
interface_task.begin();
#if SK_DISPLAY
display_task.begin();
vTaskDelete(nullptr);
// Connect display to motor_task's knob state feed
motor_task.addListener(display_task.getKnobStateQueue());
#endif
// Create a queue and register it with motor_task to print knob state to serial (see loop() below)
knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
assert(knob_state_debug_queue != NULL);
motor_task.addListener(knob_state_debug_queue);
// Free up the loop task
vTaskDelete(NULL);
}
static KnobState state = {};
uint32_t last_debug;
void loop() {
assert(false);
// Print any new state, at most 5 times per second
if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
Serial.println(state.current_position);
last_debug = millis();
}
static uint32_t last_stack_debug;
if (millis() - last_stack_debug > 1000) {
Serial.println("Stack high water:");
Serial.printf("main: %d\n", uxTaskGetStackHighWaterMark(NULL));
#if SK_DISPLAY
Serial.printf("display: %d\n", uxTaskGetStackHighWaterMark(display_task.getHandle()));
#endif
Serial.printf("motor: %d\n", uxTaskGetStackHighWaterMark(motor_task.getHandle()));
Serial.printf("interface: %d\n", uxTaskGetStackHighWaterMark(interface_task.getHandle()));
last_stack_debug = millis();
}
}

View File

@ -1,13 +1,10 @@
#include <SimpleFOC.h>
#include <sensors/MagneticSensorI2C.h>
#include "motor_task.h"
#include "mt6701_sensor.h"
#include "tlv_sensor.h"
template <typename T> T CLAMP(const T& value, const T& low, const T& high)
{
return value < low ? low : (value > high ? high : value);
}
#include "util.h"
static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
static const float DEAD_ZONE_RAD = 1 * _PI / 180;
@ -19,8 +16,8 @@ static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
queue_ = xQueueCreate(1, sizeof(KnobConfig));
MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 1200, 1, task_core) {
queue_ = xQueueCreate(5, sizeof(Command));
assert(queue_ != NULL);
}
@ -28,11 +25,15 @@ MotorTask::~MotorTask() {}
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
TlvSensor tlv = TlvSensor();
BLDCMotor motor = BLDCMotor(1);
BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
#if SENSOR_TLV
TlvSensor encoder = TlvSensor();
#elif SENSOR_MT6701
MT6701Sensor encoder = MT6701Sensor();
#endif
// MagneticSensorI2C tlv = MagneticSensorI2C(AS5600_I2C);
Commander command = Commander(Serial);
@ -40,18 +41,41 @@ Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }
void MotorTask::run() {
// Hardware-specific configuration:
// TODO: make this easier to configure
// Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
// So this value is based on experimentation.
// TODO: dig into SimpleFOC calibration and find/fix the issue
// float zero_electric_offset = -0.6; // original proto
//float zero_electric_offset = 0.4; // handheld 1
// float zero_electric_offset = -0.8; // handheld 2
// float zero_electric_offset = 2.93; //0.15; // 17mm test
// float zero_electric_offset = 0.66; // 15mm handheld
float zero_electric_offset = 7.34;
Direction foc_direction = Direction::CW;
motor.pole_pairs = 7;
driver.voltage_power_supply = 5;
driver.init();
Wire.begin();
Wire.setClock(400000);
tlv.init();
#if SENSOR_TLV
encoder.init(Wire, false);
#endif
#if SENSOR_MT6701
encoder.init();
// motor.LPF_angle = LowPassFilter(0.05);
#endif
// motor.LPF_current_q = {0.01};
motor.linkDriver(&driver);
motor.controller = MotionControlType::torque;
motor.voltage_limit = 5;
motor.linkSensor(&tlv);
motor.velocity_limit = 10000;
motor.linkSensor(&encoder);
// Not actually using the velocity loop; but I'm using those PID variables
// because SimpleFOC studio supports updating them easily over serial for tuning.
@ -66,16 +90,191 @@ void MotorTask::run() {
motor.init();
tlv.update();
encoder.update();
delay(10);
// Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
// So this value is based on experimentation.
// TODO: dig into SimpleFOC calibration and find/fix the issue
float zero_electric_offset = -0.6;
motor.initFOC(zero_electric_offset, Direction::CCW);
motor.initFOC(zero_electric_offset, foc_direction);
bool calibrate = false;
Serial.println("Press Y to run calibration");
uint32_t t = millis();
while (millis() - t < 3000) {
if (Serial.read() == 'Y') {
calibrate = true;
break;
}
delay(10);
}
if (calibrate) {
motor.controller = MotionControlType::angle_openloop;
motor.pole_pairs = 1;
motor.initFOC(0, Direction::CW);
float a = 0;
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
motor.move(a);
delay(1);
}
float start_sensor = encoder.getAngle();
for (; a < 3 * _2PI; a += 0.01) {
encoder.update();
motor.move(a);
delay(1);
}
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
delay(1);
}
float end_sensor = encoder.getAngle();
motor.voltage_limit = 0;
motor.move(a);
// Serial.println("Did motor turn counterclockwise? Press Y to continue, otherwise change motor wiring and restart");
// while (Serial.read() != 'Y') {
// delay(10);
// }
Serial.println();
// TODO: check for no motor movement!
Serial.print("Sensor measures positive for positive motor rotation: ");
if (end_sensor > start_sensor) {
Serial.println("YES, Direction=CW");
motor.initFOC(0, Direction::CW);
} else {
Serial.println("NO, Direction=CCW");
motor.initFOC(0, Direction::CCW);
}
// Rotate many electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
uint8_t electrical_revolutions = 20;
Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
motor.voltage_limit = 5;
motor.move(a);
Serial.println("Going to electrical zero...");
float destination = a + _2PI;
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
}
Serial.println("pause...");
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
delay(1);
}
Serial.println("Measuring...");
start_sensor = motor.sensor_direction * encoder.getAngle();
destination = a + electrical_revolutions * _2PI;
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
}
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
motor.move(a);
delay(1);
}
end_sensor = motor.sensor_direction * encoder.getAngle();
motor.voltage_limit = 0;
motor.move(a);
if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
Serial.println("ERROR: motor did not reach target!");
while(1) {}
}
float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
Serial.println(electrical_per_mechanical);
int measured_pole_pairs = (int)round(electrical_per_mechanical);
Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
delay(1000);
// Measure mechanical angle at every electrical zero for several revolutions
motor.voltage_limit = 5;
motor.move(a);
float offset_x = 0;
float offset_y = 0;
float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
float destination2 = (floor(a / _2PI)) * _2PI;
for (; a < destination1; a += 0.4) {
motor.move(a);
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
float offset_angle = measured_electrical_angle - real_electrical_angle;
offset_x += cosf(offset_angle);
offset_y += sinf(offset_angle);
Serial.print(degrees(real_electrical_angle));
Serial.print(", ");
Serial.print(degrees(measured_electrical_angle));
Serial.print(", ");
Serial.println(degrees(_normalizeAngle(offset_angle)));
}
for (; a > destination2; a -= 0.4) {
motor.move(a);
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
float offset_angle = measured_electrical_angle - real_electrical_angle;
offset_x += cosf(offset_angle);
offset_y += sinf(offset_angle);
Serial.print(degrees(real_electrical_angle));
Serial.print(", ");
Serial.print(degrees(measured_electrical_angle));
Serial.print(", ");
Serial.println(degrees(_normalizeAngle(offset_angle)));
}
motor.voltage_limit = 0;
motor.move(a);
float avg_offset_angle = atan2f(offset_y, offset_x);
// Apply settings
motor.pole_pairs = measured_pole_pairs;
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
motor.voltage_limit = 5;
motor.controller = MotionControlType::torque;
Serial.print("\n\nRESULTS:\n zero electric angle: ");
Serial.println(motor.zero_electric_angle);
Serial.print(" direction: ");
if (motor.sensor_direction == Direction::CW) {
Serial.println("CW");
} else {
Serial.println("CCW");
}
Serial.printf(" pole pairs: %d\n", motor.pole_pairs);
delay(2000);
}
Serial.println(motor.zero_electric_angle);
command.add('M', &doMotor, "foo");
@ -96,33 +295,59 @@ void MotorTask::run() {
uint32_t last_idle_start = 0;
uint32_t last_debug = 0;
uint32_t last_display_update = 0;
uint32_t last_publish = 0;
while (1) {
motor.loopFOC();
if (xQueueReceive(queue_, &config, 0) == pdTRUE) {
Serial.println("Got new config");
current_detent_center = motor.shaft_angle;
Command command;
if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
switch (command.command_type) {
case CommandType::CONFIG: {
config = command.data.config;
Serial.println("Got new config");
current_detent_center = motor.shaft_angle;
#if SK_INVERT_ROTATION
current_detent_center = -motor.shaft_angle;
#endif
// Update derivative factor of torque controller based on detent width.
// If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
// This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
// Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
// for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
// get runaway due to sensor noise & lag)).
// TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
// direction) whenever the position changes when the detent width is too small for the P factor to work well.
const float derivative_lower_strength = config.detent_strength_unit * 0.04;
const float derivative_upper_strength = config.detent_strength_unit * 0;
const float derivative_position_width_lower = 5 * PI / 180;
const float derivative_position_width_upper = 10 * PI / 180;
const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
motor.PID_velocity.D = CLAMP(
raw,
min(derivative_lower_strength, derivative_upper_strength),
max(derivative_lower_strength, derivative_upper_strength)
);
// Update derivative factor of torque controller based on detent width.
// If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
// This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
// Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
// for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
// get runaway due to sensor noise & lag)).
// TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
// direction) whenever the position changes when the detent width is too small for the P factor to work well.
const float derivative_lower_strength = config.detent_strength_unit * 0.08;
const float derivative_upper_strength = config.detent_strength_unit * 0.02;
const float derivative_position_width_lower = radians(3);
const float derivative_position_width_upper = radians(8);
const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
motor.PID_velocity.D = CLAMP(
raw,
min(derivative_lower_strength, derivative_upper_strength),
max(derivative_lower_strength, derivative_upper_strength)
);
break;
}
case CommandType::HAPTIC: {
float strength = command.data.haptic.press ? 5 : 1.5;
motor.move(strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(-strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(0);
motor.loopFOC();
break;
}
}
}
idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
@ -147,6 +372,9 @@ void MotorTask::run() {
}
float angle_to_detent_center = motor.shaft_angle - current_detent_center;
#if SK_INVERT_ROTATION
angle_to_detent_center = -motor.shaft_angle - current_detent_center;
#endif
if (angle_to_detent_center > config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position > 0)) {
current_detent_center += config.position_width_radians;
angle_to_detent_center -= config.position_width_radians;
@ -168,27 +396,63 @@ void MotorTask::run() {
if (fabsf(motor.shaft_velocity) > 20) {
if (fabsf(motor.shaft_velocity) > 60) {
// Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
motor.move(0);
} else {
motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
float torque = motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment);
#if SK_INVERT_ROTATION
torque = -torque;
#endif
motor.move(torque);
}
if (millis() - last_display_update > 10) {
display_task_.setData({
if (millis() - last_publish > 10) {
publish({
.current_position = config.position,
.sub_position_unit = -angle_to_detent_center / config.position_width_radians,
.config = config,
});
last_display_update = millis();
last_publish = millis();
}
motor.monitor();
// command.run();
delay(1);
}
}
void MotorTask::setConfig(const KnobConfig& config) {
xQueueOverwrite(queue_, &config);
Command command = {
.command_type = CommandType::CONFIG,
.data = {
.config = config,
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::playHaptic(bool press) {
Command command = {
.command_type = CommandType::HAPTIC,
.data = {
.haptic = {
.press = press,
},
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::addListener(QueueHandle_t queue) {
listeners_.push_back(queue);
}
void MotorTask::publish(const KnobState& state) {
for (auto listener : listeners_) {
xQueueOverwrite(listener, &state);
}
}

View File

@ -1,24 +1,49 @@
#pragma once
#include <Arduino.h>
#include <vector>
#include "knob_data.h"
#include "task.h"
#include "display_task.h"
enum class CommandType {
CONFIG,
HAPTIC,
};
struct HapticData {
bool press;
};
struct Command {
CommandType command_type;
union CommandData {
KnobConfig config;
HapticData haptic;
};
CommandData data;
};
class MotorTask : public Task<MotorTask> {
friend class Task<MotorTask>; // Allow base Task to invoke protected run()
public:
MotorTask(const uint8_t task_core, DisplayTask& display_task);
MotorTask(const uint8_t task_core);
~MotorTask();
void setConfig(const KnobConfig& config);
void playHaptic(bool press);
void addListener(QueueHandle_t queue);
protected:
void run();
private:
DisplayTask& display_task_;
QueueHandle_t queue_;
std::vector<QueueHandle_t> listeners_;
void publish(const KnobState& state);
};

View File

@ -0,0 +1,118 @@
#include "mt6701_sensor.h"
#include "driver/spi_master.h"
static const float ALPHA = 0.4;
static uint8_t tableCRC6[64] = {
0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09,
0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11,
0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39,
0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21,
0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A,
0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32,
0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A,
0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02
};
/*32-bit input data, right alignment, Calculation over 18 bits (mult. of 6) */
static uint8_t CRC6_43_18bit (uint32_t w_InputData)
{
uint8_t b_Index = 0;
uint8_t b_CRC = 0;
b_Index = (uint8_t )(((uint32_t)w_InputData >> 12u) & 0x0000003Fu);
b_CRC = (uint8_t )(((uint32_t)w_InputData >> 6u) & 0x0000003Fu);
b_Index = b_CRC ^ tableCRC6[b_Index];
b_CRC = (uint8_t )((uint32_t)w_InputData & 0x0000003Fu);
b_Index = b_CRC ^ tableCRC6[b_Index];
b_CRC = tableCRC6[b_Index];
return b_CRC;
}
MT6701Sensor::MT6701Sensor() {}
void MT6701Sensor::init() {
pinMode(PIN_MT_CSN, OUTPUT);
digitalWrite(PIN_MT_CSN, HIGH);
spi_bus_config_t tx_bus_config = {
.mosi_io_num = -1,
.miso_io_num = PIN_MT_DATA,
.sclk_io_num = PIN_MT_CLOCK,
.quadwp_io_num = -1,
.quadhd_io_num = -1,
.max_transfer_sz = 1000,
};
esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
ESP_ERROR_CHECK(ret);
spi_device_interface_config_t tx_device_config = {
.command_bits=0,
.address_bits=0,
.dummy_bits=0,
.mode=1,
.duty_cycle_pos=0,
.cs_ena_pretrans=4,
.cs_ena_posttrans=0,
.clock_speed_hz=4000000,
.input_delay_ns=0,
.spics_io_num=PIN_MT_CSN,
.flags = 0,
.queue_size=1,
.pre_cb=NULL,
.post_cb=NULL,
};
ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
ESP_ERROR_CHECK(ret);
spi_transaction_.flags = SPI_TRANS_USE_RXDATA;
spi_transaction_.length = 24;
spi_transaction_.rxlength = 24;
spi_transaction_.tx_buffer = NULL;
spi_transaction_.rx_buffer = NULL;
}
float MT6701Sensor::getSensorAngle() {
uint32_t now = micros();
if (now - last_update_ > 100) {
esp_err_t ret=spi_device_polling_transmit(spi_device_, &spi_transaction_);
assert(ret==ESP_OK);
uint32_t spi_32 = (spi_transaction_.rx_data[0] << 16) | (spi_transaction_.rx_data[1] << 8) | spi_transaction_.rx_data[2];
uint32_t angle_spi = spi_32 >> 10;
uint8_t field_status = (spi_32 >> 6) & 0x3;
uint8_t push_status = (spi_32 >> 8) & 0x1;
uint8_t loss_status = (spi_32 >> 9) & 0x1;
uint8_t received_crc = spi_32 & 0x3F;
uint8_t calculated_crc = CRC6_43_18bit(spi_32 >> 6);
if (received_crc == calculated_crc) {
float new_angle = (float)angle_spi * 2 * PI / 16384;
float new_x = cosf(new_angle);
float new_y = sinf(new_angle);
x_ = new_x * ALPHA + x_ * (1-ALPHA);
y_ = new_y * ALPHA + y_ * (1-ALPHA);
} else {
Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
}
last_update_ = now;
}
float rad = -atan2f(y_, x_);
if (rad < 0) {
rad += 2*PI;
}
return rad;
}

View File

@ -0,0 +1,27 @@
#pragma once
#include <SimpleFOC.h>
#include "driver/spi_master.h"
class MT6701Sensor : public Sensor {
public:
MT6701Sensor();
// initialize the sensor hardware
void init();
// Get current shaft angle from the sensor hardware, and
// return it as a float in radians, in the range 0 to 2PI.
// - This method is pure virtual and must be implemented in subclasses.
// Calling this method directly does not update the base-class internal fields.
// Use update() when calling from outside code.
float getSensorAngle();
private:
spi_device_handle_t spi_device_;
spi_transaction_t spi_transaction_ = {};
float x_;
float y_;
uint32_t last_update_;
};

View File

@ -1,11 +1,13 @@
#include "tlv_sensor.h"
static const float ALPHA = 0.04;
static const float ALPHA = 1;
TlvSensor::TlvSensor() {}
void TlvSensor::init() {
tlv_.begin();
void TlvSensor::init(TwoWire* wire, bool invert) {
wire_ = wire;
invert_ = invert;
tlv_.begin(*wire);
tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
tlv_.disableInterrupt();
tlv_.disableTemp();
@ -13,13 +15,35 @@ void TlvSensor::init() {
float TlvSensor::getSensorAngle() {
uint32_t now = micros();
if (now - last_update_ > 100) {
if (now - last_update_ > 50) {
tlv_.updateData();
frame_counts_[cur_frame_count_index_] = tlv_.getExpectedFrameCount();
cur_frame_count_index_++;
if (cur_frame_count_index_ >= sizeof(frame_counts_)) {
cur_frame_count_index_ = 0;
}
x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
last_update_ = now;
bool all_same = true;
uint8_t match_frame = frame_counts_[0];
for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
if (frame_counts_[i] != match_frame) {
all_same = false;
break;
}
}
if (all_same) {
Serial.println("LOCKED!");
init(wire_, invert_);
// Force unique frame counts to avoid reset loop
for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
frame_counts_[i] = i;
}
}
}
float rad = atan2f(y_, x_);
float rad = (invert_ ? -1 : 1) * atan2f(y_, x_);
if (rad < 0) {
rad += 2*PI;
}

View File

@ -8,7 +8,7 @@ class TlvSensor : public Sensor {
TlvSensor();
// initialize the sensor hardware
void init();
void init(TwoWire* wire, bool invert);
// Get current shaft angle from the sensor hardware, and
// return it as a float in radians, in the range 0 to 2PI.
@ -21,4 +21,9 @@ class TlvSensor : public Sensor {
float x_;
float y_;
uint32_t last_update_;
TwoWire* wire_;
bool invert_;
uint8_t frame_counts_[3] = {};
uint8_t cur_frame_count_index_ = 0;
};

7
firmware/src/util.h Normal file
View File

@ -0,0 +1,7 @@
#pragma once
template <typename T> T CLAMP(const T& value, const T& low, const T& high)
{
return value < low ? low : (value > high ? high : value);
}