diff --git a/CMakeLists.txt b/CMakeLists.txt index f282769e5..58cd77f02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,8 @@ add_library(daisy STATIC ${MODULE_DIR}/dev/lcd_hd44780.cpp ${MODULE_DIR}/dev/sdram.cpp ${MODULE_DIR}/dev/sr_595.cpp + ${MODULE_DIR}/dev/trill/Trill.cpp + ${MODULE_DIR}/dev/trill/CentroidDetection.cpp ${MODULE_DIR}/hid/audio.cpp ${MODULE_DIR}/hid/ctrl.cpp ${MODULE_DIR}/hid/encoder.cpp diff --git a/Makefile b/Makefile index 664a0811f..8c4c2ac1b 100644 --- a/Makefile +++ b/Makefile @@ -35,6 +35,8 @@ dev/codec_ak4556 \ dev/codec_pcm3060 \ dev/codec_wm8731 \ dev/lcd_hd44780 \ +dev/trill/Trill \ +dev/trill/CentroidDetection \ dev/sdram \ hid/ctrl \ hid/encoder \ diff --git a/examples/Trill-Craft/Makefile b/examples/Trill-Craft/Makefile new file mode 100644 index 000000000..5f5b1b2ef --- /dev/null +++ b/examples/Trill-Craft/Makefile @@ -0,0 +1,12 @@ +# Project Name +TARGET = Trill + +# Sources +CPP_SOURCES = Trill-Craft.cpp + +# Library Locations +LIBDAISY_DIR = ../.. + +# Core location, and generic Makefile. +SYSTEM_FILES_DIR = $(LIBDAISY_DIR)/core +include $(SYSTEM_FILES_DIR)/Makefile diff --git a/examples/Trill-Craft/Trill-Craft.cpp b/examples/Trill-Craft/Trill-Craft.cpp new file mode 100644 index 000000000..5369dbf71 --- /dev/null +++ b/examples/Trill-Craft/Trill-Craft.cpp @@ -0,0 +1,41 @@ +/** + * Read Trill craft capacitive sensor + */ + +#include "daisy_seed.h" +#include "dev/trill/Trill.h" + +using namespace daisy; + +DaisySeed hw; + +int main(void) +{ + // Initialize the Daisy Seed + hw.Init(); + + // Start the Serial Logger + hw.StartLog(); + + // Create a Trill object + Trill trill; + + // Initialize the Trill object + int i2cBus = 1; // only 1 and 4 are properly mapped to pins on the Seed + int ret = trill.setup(i2cBus, Trill::CRAFT); + if(ret) + hw.PrintLine("trill.setup() returned %d", ret); + + // loop forever + while(1) + { + hw.DelayMs(100); + trill.readI2C(); + for(auto &x : trill.rawData) + { + hw.Print("%d ", int(x * 100000.f)); + } + hw.PrintLine(""); + } + return 0; +} diff --git a/examples/Trill/Makefile b/examples/Trill/Makefile new file mode 100644 index 000000000..e08c34b32 --- /dev/null +++ b/examples/Trill/Makefile @@ -0,0 +1,12 @@ +# Project Name +TARGET = Trill + +# Sources +CPP_SOURCES = Trill.cpp + +# Library Locations +LIBDAISY_DIR = ../.. + +# Core location, and generic Makefile. +SYSTEM_FILES_DIR = $(LIBDAISY_DIR)/core +include $(SYSTEM_FILES_DIR)/Makefile diff --git a/examples/Trill/Trill.cpp b/examples/Trill/Trill.cpp new file mode 100644 index 000000000..1d54bb77f --- /dev/null +++ b/examples/Trill/Trill.cpp @@ -0,0 +1,52 @@ +/** + * Read Trill capacitive sensor + */ + +#include "daisy_seed.h" +#include "dev/trill/Trill.h" + +using namespace daisy; +using namespace daisy::seed; + +DaisySeed hw; + +void AudioCallback(AudioHandle::InputBuffer in, + AudioHandle::OutputBuffer out, + size_t size) +{ + // +} + +int main(void) +{ + // Initialize the Daisy Seed + hw.Init(); + + // Start the Serial Logger + hw.StartLog(); + + /** Start the Audio engine, and call the "AudioCallback" function to fill new data */ + hw.StartAudio(AudioCallback); + + // Create a Trill object + Trill trill; + + // Initialize the Trill object + int i2cBus = 1; // only 1 and 4 are properly mapped to pins on the Seed + int ret = trill.setup(i2cBus, Trill::BAR); + if(ret) + hw.Print("trill.setup() returned %d\n", ret); + + // loop forever + while(1) + { + trill.readI2C(); + if(trill.getNumTouches()) + { + for(size_t n = 0; n < trill.getNumTouches(); ++n) + hw.Print( + "%.3f (%.2f) ", trill.touchLocation(n), trill.touchSize(n)); + hw.Print("\n"); + } + } +} diff --git a/src/dev/trill/CentroidDetection.cpp b/src/dev/trill/CentroidDetection.cpp new file mode 100644 index 000000000..e43df1372 --- /dev/null +++ b/src/dev/trill/CentroidDetection.cpp @@ -0,0 +1,261 @@ +#include "CentroidDetection.h" + +// a small helper class, whose main purpose is to wrap the #include +// and make all the variables related to it private and multi-instance safe +class CentroidDetection::CalculateCentroids +{ + public: + typedef CentroidDetection::WORD WORD; + typedef uint8_t BYTE; + typedef uint8_t BOOL; + WORD* CSD_waSnsDiff; + WORD wMinimumCentroidSize = 0; + BYTE SLIDER_BITS = 16; + WORD wAdjacentCentroidNoiseThreshold + = 400; // Trough between peaks needed to identify two centroids + // calculateCentroids is defined here: +#include "calculateCentroids.h" + void processCentroids(WORD* wVCentroid, + WORD* wVCentroidSize, + BYTE MAX_NUM_CENTROIDS, + BYTE FIRST_SENSOR_V, + BYTE LAST_SENSOR_V, + BYTE numSensors) + { + long temp; + BYTE lastActiveSensor; + BYTE counter; + WORD posEndOfLoop = (LAST_SENSOR_V - FIRST_SENSOR_V) << SLIDER_BITS; + temp = calculateCentroids(wVCentroid, + wVCentroidSize, + MAX_NUM_CENTROIDS, + FIRST_SENSOR_V, + LAST_SENSOR_V, + numSensors); // Vertical centroids + lastActiveSensor = temp >> 8; + + temp = lastActiveSensor + - (LAST_SENSOR_V + - FIRST_SENSOR_V); // retrieve the (wrapped) index + //check for activity in the wraparound area + // IF the last centroid ended after wrapping around ... + // AND the first centroid was located before the end of the last ... + if(lastActiveSensor != 255 // 255 means no active sensor + && lastActiveSensor >= LAST_SENSOR_V - FIRST_SENSOR_V + && ((unsigned)temp) << SLIDER_BITS >= wVCentroid[0]) + { + // THEN the last touch is used to replace the first one + for(counter = MAX_NUM_CENTROIDS - 1; counter >= 1; counter--) + { + if(0xFFFF == wVCentroid[counter]) + continue; + // replace the first centroid + wVCentroidSize[0] = wVCentroidSize[counter]; + wVCentroid[0] = wVCentroid[counter]; + // wrap around the position if needed + if(wVCentroid[0] >= posEndOfLoop) + wVCentroid[0] -= posEndOfLoop; + // discard the last centroid + wVCentroid[counter] = 0xFFFF; + wVCentroidSize[counter] = 0x0; + break; + } + } + } +}; + +CentroidDetection::CentroidDetection(unsigned int numReadings, + unsigned int maxNumCentroids, + float sizeScale) +{ + setup(numReadings, maxNumCentroids, sizeScale); +} + +CentroidDetection::CentroidDetection(const std::vector& order, + unsigned int maxNumCentroids, + float sizeScale) +{ + setup(order, maxNumCentroids, sizeScale); +} + +int CentroidDetection::setup(unsigned int numReadings, + unsigned int maxNumCentroids, + float sizeScale) +{ + std::vector order; + for(unsigned int n = 0; n < numReadings; ++n) + order.push_back(n); + return setup(order, maxNumCentroids, sizeScale); +} + +int CentroidDetection::setup(const std::vector& order, + unsigned int maxNumCentroids, + float sizeScale) +{ + this->order = order; + setWrapAround(0); + this->maxNumCentroids = maxNumCentroids; + centroidBuffer.resize(maxNumCentroids); + sizeBuffer.resize(maxNumCentroids); + centroids.resize(maxNumCentroids); + sizes.resize(maxNumCentroids); + data.resize(order.size()); + setSizeScale(sizeScale); + setNoiseThreshold(0); + cc = std::shared_ptr(new CalculateCentroids()); + setMultiplierBits(cc->SLIDER_BITS); + num_touches = 0; + return 0; +} + +void CentroidDetection::setWrapAround(unsigned int n) +{ + num_sensors = order.size() + n; +} + +void CentroidDetection::setMultiplierBits(unsigned int n) +{ + cc->SLIDER_BITS = n; + locationScale = 1.f / ((order.size() - 1) * (1 << cc->SLIDER_BITS)); +} + +void CentroidDetection::process(const DATA_T* rawData) +{ + for(unsigned int n = 0; n < order.size(); ++n) + { + float val = rawData[order[n]] * (1 << 12); + val -= noiseThreshold; + if(val < 0) + val = 0; + data[n] = val; + } + cc->CSD_waSnsDiff = data.data(); + cc->processCentroids(centroidBuffer.data(), + sizeBuffer.data(), + maxNumCentroids, + 0, + order.size(), + num_sensors); + + // Look for 1st instance of 0xFFFF (no touch) in the buffer + unsigned int i; + for(i = 0; i < centroidBuffer.size(); ++i) + { + if(0xffff == centroidBuffer[i]) + break; // at the first non-touch, break + centroids[i] = centroidBuffer[i] * locationScale; + sizes[i] = sizeBuffer[i] * sizeScale; + } + num_touches = i; +} + +void CentroidDetection::setSizeScale(float sizeScale) +{ + this->sizeScale = 1.f / sizeScale; +} + +void CentroidDetection::setMinimumTouchSize(DATA_T minSize) +{ + cc->wMinimumCentroidSize = minSize; +} + +void CentroidDetection::setNoiseThreshold(DATA_T threshold) +{ + noiseThreshold = threshold; +} + +unsigned int CentroidDetection::getNumTouches() const +{ + return num_touches; +} + +CentroidDetection::DATA_T +CentroidDetection::touchLocation(unsigned int touch_num) const +{ + if(touch_num < maxNumCentroids) + return centroids[touch_num]; + else + return 0; +} + +CentroidDetection::DATA_T +CentroidDetection::touchSize(unsigned int touch_num) const +{ + if(touch_num < num_touches) + return sizes[touch_num]; + else + return 0; +} + +// code below from Trill.cpp + +#define compoundTouch(LOCATION, SIZE, TOUCHES) \ + { \ + float avg = 0; \ + float totalSize = 0; \ + unsigned int numTouches = TOUCHES; \ + for(unsigned int i = 0; i < numTouches; i++) \ + { \ + avg += LOCATION(i) * SIZE(i); \ + totalSize += SIZE(i); \ + } \ + if(numTouches) \ + avg = avg / totalSize; \ + return avg; \ + } + +CentroidDetection::DATA_T CentroidDetection::compoundTouchLocation() const +{ + compoundTouch(touchLocation, touchSize, getNumTouches()); +} + +CentroidDetection::DATA_T CentroidDetection::compoundTouchSize() const +{ + float size = 0; + for(unsigned int i = 0; i < getNumTouches(); i++) + size += touchSize(i); + return size; +} + +void CentroidDetectionScaled::setUsableRange(DATA_T min, DATA_T max) +{ + this->min = min; + this->max = max; +} + +static inline float +map(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +static inline float constrain(float x, float min_val, float max_val) +{ + if(x < min_val) + return min_val; + if(x > max_val) + return max_val; + return x; +} + +static inline float mapAndConstrain(float x, + float in_min, + float in_max, + float out_min, + float out_max) +{ + float value = map(x, in_min, in_max, out_min, out_max); + value = constrain(value, out_min, out_max); + return value; +} + +void CentroidDetectionScaled::process(const DATA_T* rawData) +{ + CentroidDetection::process(rawData); + size_t numTouches = getNumTouches(); + for(size_t n = 0; n < numTouches; ++n) + { + centroids[n] = mapAndConstrain(centroids[n], min, max, 0, 1); + sizes[n] = std::min(sizes[n], 1.f); + } +} diff --git a/src/dev/trill/CentroidDetection.h b/src/dev/trill/CentroidDetection.h new file mode 100644 index 000000000..1ccba9ad7 --- /dev/null +++ b/src/dev/trill/CentroidDetection.h @@ -0,0 +1,76 @@ +#pragma once +#include +#include +#include + +class CentroidDetection +{ + public: + typedef float DATA_T; + CentroidDetection(){}; + CentroidDetection(unsigned int numReadings, + unsigned int maxNumCentroids, + float sizeScale); + CentroidDetection(const std::vector& order, + unsigned int maxNumCentroids, + float sizeScale); + int setup(unsigned int numReadings, + unsigned int maxNumCentroids, + float sizeScale); + int setup(const std::vector& order, + unsigned int maxNumCentroids, + float sizeScale); + void process(const DATA_T* rawData); + void setSizeScale(float sizeScale); + void setMinimumTouchSize(DATA_T minSize); + void setNoiseThreshold(DATA_T threshold); + /** + * Set how many of the values at the beginning of `rawData` can be + * joined in a single centroid with those at the end of `rawData` if a + * centroid is detected across them. + * Useful for ring-like devices. + */ + void setWrapAround(unsigned int n); + /** + * How many extra bits to use during the fixed-point multiplication + * that computes the centroids position. Defaults to 7. + */ + void setMultiplierBits(unsigned int n); + unsigned int getNumTouches() const; + DATA_T touchLocation(unsigned int touch_num) const; + DATA_T touchSize(unsigned int touch_num) const; + DATA_T compoundTouchLocation() const; + DATA_T compoundTouchSize() const; + + private: + typedef uint32_t WORD; + class CalculateCentroids; + + protected: + std::vector centroids; + std::vector sizes; + + private: + std::vector centroidBuffer; + std::vector sizeBuffer; + unsigned int maxNumCentroids; + std::vector order; + unsigned int num_sensors; + std::vector data; + float sizeScale; + float locationScale; + std::shared_ptr cc; + unsigned int num_touches; + DATA_T noiseThreshold; +}; + +class CentroidDetectionScaled : public CentroidDetection +{ + public: + void setUsableRange(DATA_T min, DATA_T max); + void process(const DATA_T* rawData); + + private: + float min = 0; + float max = 1; +}; diff --git a/src/dev/trill/I2c.h b/src/dev/trill/I2c.h new file mode 100644 index 000000000..8f491d736 --- /dev/null +++ b/src/dev/trill/I2c.h @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include "per/i2c.h" +typedef unsigned char i2c_char_t; + +class I2c +{ + protected: + ssize_t readBytes(void *buf, size_t count); + ssize_t writeBytes(const void *buf, size_t count); + enum + { + kTimeout = 10 + }; + int i2cAddress; + + public: + I2c(){}; + I2c(I2c &&) = delete; + int initI2C_RW(int bus, int address, int dummy); + int closeI2C(); + + virtual ~I2c(); + daisy::I2CHandle i2cHandle; +}; + +inline int I2c::initI2C_RW(int bus, int address, int dummy) +{ + using namespace daisy; + I2CHandle::Config cfg; + cfg.mode = I2CHandle::Config::Mode::I2C_MASTER; + cfg.speed = I2CHandle::Config::Speed::I2C_400KHZ; + cfg.periph = (I2CHandle::Config::Peripheral)bus; + // TODO: default pins should be handled by I2CHandle + switch(bus) + { + default: + // can't easily find what the default pins for the other busses are + // meant to be, so we use the default. + // TODO: how do we log from here? + case 1: + cfg.periph = I2CHandle::Config::Peripheral::I2C_1; + cfg.pin_config.scl = {PORTB, 8}; + cfg.pin_config.sda = {PORTB, 9}; + break; + case 4: + cfg.periph = I2CHandle::Config::Peripheral::I2C_4; + cfg.pin_config.scl = {PORTB, 6}; + cfg.pin_config.sda = {PORTB, 7}; + break; + } + cfg.address = i2cAddress; // this seems unused anyhow + i2cAddress = address; + i2cHandle.Init(cfg); + return 0; +} + +inline int I2c::closeI2C() +{ + return 0; +} + +inline ssize_t I2c::readBytes(void *buf, size_t count) +{ + if(daisy::I2CHandle::Result::OK + == i2cHandle.ReceiveBlocking( + i2cAddress, (uint8_t *)buf, count, kTimeout)) + return count; + else + return -1; +} + +inline ssize_t I2c::writeBytes(const void *buf, size_t count) +{ + if(daisy::I2CHandle::Result::OK + == i2cHandle.TransmitBlocking( + i2cAddress, (uint8_t *)buf, count, kTimeout)) + return count; + else + return -1; +} + +inline I2c::~I2c() {} + +#include "sys/system.h" +inline int usleep(useconds_t us) +{ + daisy::System::Delay((us + 999) / 1000); + return 0; +} diff --git a/src/dev/trill/Trill.cpp b/src/dev/trill/Trill.cpp new file mode 100644 index 000000000..6e7196c02 --- /dev/null +++ b/src/dev/trill/Trill.cpp @@ -0,0 +1,1131 @@ +#include "Trill.h" +#include +#include +#include + +#ifndef __try +#if __cpp_exceptions +#define __try try +#define __catch(X) catch(X) +#define __throw_exception_again throw +#else +#define __try if(true) +#define __catch(X) if(false) +#define __throw_exception_again +#endif +#endif // !__try + +constexpr uint8_t Trill::speedValues[4]; +#define MAX_TOUCH_1D_OR_2D \ + (((device_type_ == SQUARE || device_type_ == HEX) ? kMaxTouchNum2D \ + : kMaxTouchNum1D)) + +enum +{ + kCentroidLengthDefault = 20, + kCentroidLengthRing = 24, + kCentroidLength2D = 32, + kRawLength = 60, +}; + +enum +{ + kCommandNone = 0, + kCommandMode = 1, + kCommandScanSettings = 2, + kCommandPrescaler = 3, + kCommandNoiseThreshold = 4, + kCommandIdac = 5, + kCommandBaselineUpdate = 6, + kCommandMinimumSize = 7, + kCommandEventMode = 9, + kCommandChannelMaskLow = 10, + kCommandChannelMaskHigh = 11, + kCommandReset = 12, + kCommandFormat = 13, + kCommandTimerPeriod = 14, + kCommandScanTrigger = 15, + kCommandAutoScanInterval = 16, + kCommandAck = 254, + kCommandIdentify = 255 +}; + +enum +{ + kOffsetCommand = 0, + kOffsetStatusByte = 3, + kOffsetChannelData = 4, +}; + +enum +{ + kNumChannelsBar = 26, + kNumChannelsRing = 30, + kNumChannelsMax = 30, +}; + +enum +{ + kMaxTouchNum1D = 5, + kMaxTouchNum2D = 4 +}; + +struct TrillDefaults +{ + TrillDefaults(std::string name, + Trill::Mode mode, + float noiseThreshold, + uint8_t address, + uint8_t prescaler) + : name(name), + mode(mode), + noiseThreshold(noiseThreshold), + address(address), + prescaler(prescaler) + { + } + std::string name; + Trill::Mode mode; + float noiseThreshold; + uint8_t address; + int8_t prescaler; +}; + +const float defaultThreshold = 0x28 / 4096.f; +static const std::map trillDefaults = { + {Trill::NONE, TrillDefaults("No device", Trill::AUTO, 0, 0xFF, -1)}, + {Trill::UNKNOWN, TrillDefaults("Unknown device", Trill::AUTO, 0, 0xFF, -1)}, + {Trill::BAR, + TrillDefaults("Bar", Trill::CENTROID, defaultThreshold, 0x20, 2)}, + {Trill::SQUARE, + TrillDefaults("Square", Trill::CENTROID, defaultThreshold, 0x28, 1)}, + {Trill::CRAFT, + TrillDefaults("Craft", Trill::DIFF, defaultThreshold, 0x30, 1)}, + {Trill::RING, + TrillDefaults("Ring", Trill::CENTROID, defaultThreshold, 0x38, 2)}, + {Trill::HEX, + TrillDefaults("Hex", Trill::CENTROID, defaultThreshold, 0x40, 1)}, + {Trill::FLEX, TrillDefaults("Flex", Trill::CENTROID, 0.03, 0x48, 4)}, +}; + +static const std::map trillModes = { + {Trill::AUTO, "Auto"}, + {Trill::CENTROID, "Centroid"}, + {Trill::RAW, "Raw"}, + {Trill::BASELINE, "Baseline"}, + {Trill::DIFF, "Diff"}, +}; + +struct trillRescaleFactors_t +{ + float pos; + float posH; + float size; +}; + +static const std::vector trillRescaleFactors = { + {.pos = 1, .posH = 0, .size = 1}, // UNKNOWN = 0, + {.pos = 3200, .posH = 0, .size = 4566}, // BAR = 1, + {.pos = 1792, .posH = 1792, .size = 3780}, // SQUARE = 2, + {.pos = 4096, .posH = 0, .size = 1}, // CRAFT = 3, + {.pos = 3584, .posH = 0, .size = 5000}, // RING = 4, + {.pos = 1920, .posH = 1664, .size = 4000}, // HEX = 5, + {.pos = 3712, .posH = 0, .size = 1200}, // FLEX = 6, +}; + +struct TrillStatusByte +{ + uint8_t frameId : 6; + uint8_t activity : 1; + uint8_t initialised : 1; + static TrillStatusByte parse(uint8_t statusByte) + { + return *(TrillStatusByte*)(&statusByte); + } +}; +static_assert( + 1 == sizeof(TrillStatusByte), + "size and layout of TrillStatusByte must match the Trill firmware"); +static_assert( + kOffsetStatusByte + sizeof(TrillStatusByte) == kOffsetChannelData, + "Assume that channel data is available immediately after the statusByte"); + +Trill::Trill() {} + +Trill::Trill(unsigned int i2c_bus, Device device, uint8_t i2c_address) +{ + setup(i2c_bus, device, i2c_address); +} + +void Trill::updateChannelMask(uint32_t mask) +{ + channelMask = (mask & ((1 << getDefaultNumChannels()) - 1)); + numChannels = std::min(int(getDefaultNumChannels()), + __builtin_popcount(channelMask)); +} + +int Trill::setup(unsigned int i2c_bus, Device device, uint8_t i2c_address) +{ + rawData.resize(kNumChannelsMax); + address = 0; + frameId = 0; + device_type_ = NONE; + TrillDefaults defaults = trillDefaults.at(device); + + if(128 <= i2c_address) + i2c_address = defaults.address; + + if(128 <= i2c_address) + { + fprintf(stderr, + "Unknown default address for device type %s\n", + defaults.name.c_str()); + return -2; + } + if(initI2C_RW(i2c_bus, i2c_address, -1)) + { + fprintf(stderr, "Unable to initialise I2C communication\n"); + return 1; + } + // until we find out the actual, disable the version check and allow + // for silent failure of commands + enableVersionCheck = false; + reset(); // this is a time-consuming NOP for fw < 3 + + // disable scanning so communication is faster + // NOTE: ignoring return of setScanTrigger(): for fw < 3, it will + // allegedly fail for lack of ack + setScanTrigger(kScanTriggerDisabled); + if(identify() != 0) + { + fprintf(stderr, "Unable to identify device\n"); + return 2; + } + if(UNKNOWN != device && device_type_ != device) + { + fprintf(stderr, + "Wrong device type detected. `%s` was requested " + "but `%s` was detected on bus %d at address %#x(%d).\n", + defaults.name.c_str(), + trillDefaults.at(device_type_).name.c_str(), + i2c_bus, + i2c_address, + i2c_address); + device_type_ = NONE; + return -3; + } + // if the device was unknown it will have changed by now + defaults = trillDefaults.at(device_type_); + // now we have a proper version, we can check against it + enableVersionCheck = true; + + constexpr uint32_t defaultChannelMask = 0xffffffff; + if(firmware_version_ >= 3) + { + setChannelMask(defaultChannelMask); + } + else + { + // only keep track of it for internal purposes + updateChannelMask(defaultChannelMask); + } + + Mode mode = defaults.mode; + if(setMode(mode) != 0) + { + fprintf(stderr, "Unable to set mode\n"); + return 3; + } + + int8_t prescaler = defaults.prescaler; + if(prescaler >= 0) + { + if(setPrescaler(prescaler)) + { + fprintf(stderr, "Unable to set prescaler\n"); + return 8; + } + } + + if(setScanSettings(0, 12)) + { + fprintf(stderr, "Unable to set scan settings\n"); + return 7; + } + + if(updateBaseline() != 0) + { + fprintf(stderr, "Unable to update baseline\n"); + return 6; + } + + if(setNoiseThreshold(defaults.noiseThreshold)) + { + fprintf(stderr, "Unable to update baseline\n"); + return 9; + } + + address = i2c_address; + readErrorOccurred = false; + + if(setScanTrigger(kScanTriggerI2c)) + return 1; + return 0; +} + +Trill::Device Trill::probe(unsigned int i2c_bus, uint8_t i2c_address) +{ + Trill t; + if(t.initI2C_RW(i2c_bus, i2c_address, -1)) + { + return Trill::NONE; + } + if(t.identify() != 0) + { + return Trill::NONE; + } + return t.device_type_; +} + +Trill::~Trill() +{ + closeI2C(); +} + +const std::string& Trill::getNameFromDevice(Device device) +{ + __try + { + return trillDefaults.at(device).name; + } + __catch(std::exception e) { return trillDefaults.at(Device::UNKNOWN).name; } +} + +static bool strCmpIns(const std::string& str1, const std::string& str2) +{ + bool equal = true; + if(str1.size() == str2.size()) + { + for(unsigned int n = 0; n < str1.size(); ++n) + { + if(std::tolower(str1[n]) != std::tolower(str2[n])) + { + equal = false; + break; + } + } + } + else + equal = false; + return equal; +} + +Trill::Device Trill::getDeviceFromName(const std::string& name) +{ + for(auto& td : trillDefaults) + { + Device device = td.first; + const std::string& str2 = trillDefaults.at(device).name; + if(strCmpIns(name, str2)) + return Device(device); + } + return Trill::UNKNOWN; +} + +const std::string& Trill::getNameFromMode(Mode mode) +{ + __try + { + return trillModes.at(mode); + } + __catch(std::exception e) { return trillModes.at(Mode::AUTO); } +} + +Trill::Mode Trill::getModeFromName(const std::string& name) +{ + for(auto& m : trillModes) + { + const std::string& str2 = m.second; + if(strCmpIns(name, str2)) + return m.first; + } + return Trill::AUTO; +} + +// macros to automatically print method names. Using gcc-specific __PRETTY_FUNCTION__. +#define WRITE_COMMAND_BUF(data) \ + writeCommandAndHandle(data, sizeof(data), __PRETTY_FUNCTION__) +#define WRITE_COMMAND(command) \ + writeCommandAndHandle(command, __PRETTY_FUNCTION__) +#define READ_BYTES_FROM(offset, data, size) \ + readBytesFrom(offset, data, size, __PRETTY_FUNCTION__) +#define READ_BYTE_FROM(offset, byte) \ + readBytesFrom(offset, byte, __PRETTY_FUNCTION__) + +int Trill::writeCommandAndHandle(i2c_char_t command, const char* name) +{ + return writeCommandAndHandle(&command, sizeof(command), name); +} + +static void printErrno(int ret) +{ + if(-1 == ret) + fprintf(stderr, "errno %d, %s.\n", errno, strerror(errno)); +} +int Trill::writeCommandAndHandle(const i2c_char_t* data, + size_t size, + const char* name) +{ + constexpr size_t kMaxCommandBytes = 3; + if(size > kMaxCommandBytes) + { + fprintf(stderr, + "Trill: cannot write more than 3 bytes to the device\n"); + return -1; + } + i2c_char_t buf[1 + kMaxCommandBytes]; + buf[0] = kOffsetCommand; + for(size_t n = 0; n < size; ++n) + buf[n + 1] = data[n]; + int bytesToWrite = size + 1; + if(verbose) + { + printf("Writing %s :", name); + for(ssize_t n = 1; n < bytesToWrite; ++n) + printf("%d ", buf[n]); + printf("\n"); + } + int ret = writeBytes(buf, bytesToWrite); + if(ret != bytesToWrite) + { + fprintf( + stderr, + "Trill: failed to write command \"%s\"; ret: %d, errno: %d, %s.\n", + name, + ret, + errno, + strerror(errno)); + return 1; + } + currentReadOffset = buf[0]; + if(kCommandReset == buf[1]) + return usleep( + 500000); // it won't ack after reset ... (TODO: should it?) + else + return waitForAck(buf[1], name); +} + +int Trill::readBytesFrom(const uint8_t offset, + i2c_char_t& byte, + const char* name) +{ + return readBytesFrom(offset, &byte, sizeof(byte), name); +} + +int Trill::readBytesFrom(const uint8_t offset, + i2c_char_t* data, + size_t size, + const char* name) +{ + if(offset != currentReadOffset) + { + int ret = writeBytes(&offset, sizeof(offset)); + if(ret != sizeof(offset)) + { + fprintf(stderr, "%s: error while setting read offset\n", name); + printErrno(ret); + return 1; + } + currentReadOffset = offset; + usleep(commandSleepTime); + } + ssize_t bytesRead = readBytes(data, size); + if(bytesRead != ssize_t(size)) + { + fprintf(stderr, + "%s: failed to read %d bytes. ret: %d\n", + name, + size, + bytesRead); + printErrno(bytesRead); + return 1; + } + return 0; +} + +int Trill::waitForAck(const uint8_t command, const char* name) +{ + if(firmware_version_ && firmware_version_ < 3) + { + // old firmware, use old sleep time + usleep(10000); + return 0; + } + size_t bytesToRead; + if(verbose) + bytesToRead = 3; + else + bytesToRead = 1; + i2c_char_t buf[bytesToRead]; + unsigned int sleep = commandSleepTime; + unsigned int totalSleep = 0; + while(totalSleep < 200000) + { + usleep(sleep); + if(readBytesFrom(kOffsetCommand, buf, sizeof(buf), name)) + return 1; + if(kCommandAck == buf[0]) + { + // The device places the received command number in the + // second byte and a command counter in the third byte. + // If verbose, those are read and can be inspected for + // debugging purposes. + verbose&& printf("Ack'ed %d(%d) with %d %d %d\n", + command, + cmdCounter, + buf[0], + buf[1], + buf[2]); + if(verbose && (kCommandIdentify != command) + && (buf[1] != command || buf[2] != cmdCounter)) + { + printf("^^^^^ reset cmdCounter\n"); + cmdCounter = buf[2]; + } + else + { + cmdCounter++; + return 0; + } + } + verbose&& printf("sleep %d: %d %d %d\n", sleep, buf[0], buf[1], buf[2]); + totalSleep += sleep; + sleep *= 2; + if(!sleep) // avoid infinite loop in case we are told not to wait for ack + break; + } + fprintf(stderr, "%s: failed to read ack for command %d\n", name, command); + return 1; +} + +#define REQUIRE_FW_AT_LEAST(num) \ + if(enableVersionCheck && firmware_version_ < num) \ + { \ + fprintf(stderr, \ + "%s unsupported with firmware version %d, requires %d\n", \ + __PRETTY_FUNCTION__, \ + firmware_version_, \ + num); \ + return 1; \ + } + +int Trill::identify() +{ + // NOTE: ignoring return of WRITE_COMMAND(): for fw < 3, it will + // allegedly fail for lack of ack + WRITE_COMMAND(kCommandIdentify); + i2c_char_t rbuf[3]; + if(READ_BYTES_FROM(kOffsetCommand, rbuf, sizeof(rbuf))) + { + device_type_ = NONE; + return -1; + } + + // if we read back just zeros, we assume the device did not respond + if(0 == rbuf[1]) + { + device_type_ = NONE; + return -1; + } + Device readDeviceType = (Device)rbuf[1]; + // if we do not recognize the device type, we also return an error + if(trillDefaults.find(readDeviceType) == trillDefaults.end()) + { + device_type_ = NONE; + return -1; + } + device_type_ = readDeviceType; + firmware_version_ = rbuf[2]; + + return 0; +} + +void Trill::updateRescale() +{ + enum + { + kRescaleFactorsComputedAtBits = 12 + }; + float scale = (1 << (16 - numBits)) + / float(1 << (16 - kRescaleFactorsComputedAtBits)); + posRescale = 1.f / trillRescaleFactors[device_type_].pos; + posHRescale = 1.f / trillRescaleFactors[device_type_].posH; + sizeRescale = scale / trillRescaleFactors[device_type_].size; + rawRescale = 1.f / (1 << numBits); +} + +void Trill::printDetails() +{ + printf("Device type: %s (%d)\n", + getNameFromDevice(device_type_).c_str(), + deviceType()); + printf("Address: %#x\n", address); + printf("Firmware version: %d\n", firmwareVersion()); +} + +void Trill::setVerbose(int verbose) +{ + this->verbose = verbose; +} + +int Trill::setMode(Mode mode) +{ + if(AUTO == mode) + mode = trillDefaults.at(device_type_).mode; + i2c_char_t buf[] = {kCommandMode, (i2c_char_t)mode}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + mode_ = mode; + return 0; +} + +int Trill::setScanSettings(uint8_t speed, uint8_t num_bits) +{ + if(speed > 3) + speed = 3; + if(num_bits < 9) + num_bits = 9; + if(num_bits > 16) + num_bits = 16; + i2c_char_t buf[] = {kCommandScanSettings, speed, num_bits}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + numBits = num_bits; + updateRescale(); + return 0; +} + +int Trill::setPrescaler(uint8_t prescaler) +{ + i2c_char_t buf[] = {kCommandPrescaler, prescaler}; + return WRITE_COMMAND_BUF(buf); +} + +int Trill::setNoiseThreshold(float threshold) +{ + threshold = threshold * (1 << numBits); + if(threshold > 255) + threshold = 255; + if(threshold < 0) + threshold = 0; + noiseThreshold = threshold + 0.5; + i2c_char_t thByte = i2c_char_t(noiseThreshold); + i2c_char_t buf[] = {kCommandNoiseThreshold, thByte}; + return WRITE_COMMAND_BUF(buf); +} + + +int Trill::setIDACValue(uint8_t value) +{ + i2c_char_t buf[] = {kCommandIdac, value}; + return WRITE_COMMAND_BUF(buf); +} + +int Trill::setMinimumTouchSize(float minSize) +{ + uint16_t size; + float maxMinSize = (1 << 16) - 1; + if(maxMinSize + > minSize / sizeRescale) // clipping to the max value we can transmit + size = maxMinSize; + else + size = minSize / sizeRescale; + i2c_char_t buf[] = {kCommandMinimumSize, + (i2c_char_t)(size >> 8), + (i2c_char_t)(size & 0xFF)}; + return WRITE_COMMAND_BUF(buf); +} + +int Trill::setTimerPeriod(float ms) +{ + if(firmware_version_ >= 3) + { + if(ms < 0) + ms = 0; + const float kMaxMs = 255 * 255 / 32.f; + if(ms > kMaxMs) + ms = kMaxMs; + // Start from a clock period of 1ms (32 cycles of the 32kHz clock) + uint32_t period = 32; + uint32_t ticks = ms + 0.5f; // round + while(ticks > 255) + { + period *= 2; + ticks /= 2; + } + if(period > 255) + { + // shouldn't get here + fprintf(stderr, + "Trill:setTimerPeriod(): the requested %f ms cannot be " + "achieved. Using %lu instead\n", + ms, + (unsigned long)(period * ticks / 32)); + period = 255; + } + i2c_char_t buf[] + = {kCommandTimerPeriod, i2c_char_t(period), i2c_char_t(ticks)}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + } + else + { + // fw 2 had kCommandAutoScanInterval, which takes a WORD + // representing cycles of the 32kHz clock + // which was used to start the timer in one-shot mode. + uint16_t arg = ms * 32 + 0.5f; + i2c_char_t buf[] = {kCommandAutoScanInterval, + (i2c_char_t)(arg >> 8), + (i2c_char_t)(arg & 0xFF)}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + } + return 0; +} + +int Trill::setAutoScanInterval(uint16_t interval) +{ + if(setTimerPeriod(interval / 32.f)) + return 1; + if(firmware_version_ >= 3) + { + // backwards compatibility: when using v3 library with v3 fw, + // but the application was written for fw 2 + if(interval) + { + // ensure scanning on timer is enabled + ScanTriggerMode mode + = ScanTriggerMode(scanTriggerMode | kScanTriggerTimer); + if(setScanTrigger(ScanTriggerMode(mode))) + return 1; + } + } + return 0; +} + +int Trill::setScanTrigger(ScanTriggerMode mode) +{ + REQUIRE_FW_AT_LEAST(3); + scanTriggerMode = mode; + i2c_char_t buf[] = {kCommandScanTrigger, i2c_char_t(scanTriggerMode)}; + return WRITE_COMMAND_BUF(buf); +} + +int Trill::setEventMode(EventMode mode) +{ + REQUIRE_FW_AT_LEAST(3); + i2c_char_t buf[] = {kCommandEventMode, i2c_char_t(mode)}; + return WRITE_COMMAND_BUF(buf); +} + +int Trill::setChannelMask(uint32_t mask) +{ + REQUIRE_FW_AT_LEAST(3); + i2c_char_t* bMask = (i2c_char_t*)&mask; + i2c_char_t buf[] = {kCommandChannelMaskLow, bMask[0], bMask[1]}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + buf[0] = kCommandChannelMaskHigh; + buf[1] = bMask[2]; + buf[2] = bMask[3]; + if(WRITE_COMMAND_BUF(buf)) + return 1; + updateChannelMask(mask); + return 0; +} + +int Trill::setTransmissionFormat(uint8_t width, uint8_t shift) +{ + REQUIRE_FW_AT_LEAST(3); + i2c_char_t buf[] = {kCommandFormat, width, shift}; + if(WRITE_COMMAND_BUF(buf)) + return 1; + transmissionWidth = width; + transmissionRightShift = shift; + return 0; +} + +int Trill::updateBaseline() +{ + return WRITE_COMMAND(kCommandBaselineUpdate); +} + +int Trill::reset() +{ + REQUIRE_FW_AT_LEAST(3); + return WRITE_COMMAND(kCommandReset); +} + +static unsigned int bytesFromSlots(size_t numWords, size_t transmissionWidth) +{ + switch(transmissionWidth) + { + default: + case 16: return numWords * 2; + case 12: return numWords + (numWords + 1) / 2; + case 8: return numWords; + } +} +unsigned int Trill::getBytesToRead(bool includesStatusByte) +{ + size_t bytesToRead = kCentroidLengthDefault; + if(CENTROID == mode_) + { + if(device_type_ == SQUARE || device_type_ == HEX) + bytesToRead = kCentroidLength2D; + if(device_type_ == RING) + bytesToRead = kCentroidLengthRing; + } + else + { + bytesToRead = bytesFromSlots(getNumChannels(), transmissionWidth); + } + bytesToRead += sizeof(TrillStatusByte) * includesStatusByte; + return bytesToRead; +} + +int Trill::readI2C(bool shouldReadStatusByte) +{ + if(NONE == device_type_ || readErrorOccurred) + return 1; + // NOTE: to avoid being too verbose, we do not check for firmware + // version here. On fw < 3, shouldReadStatusByte will read one more + // byte full of garbage. + + ssize_t bytesToRead = getBytesToRead(shouldReadStatusByte); + dataBuffer.resize(bytesToRead); + i2c_char_t offset + = shouldReadStatusByte ? kOffsetStatusByte : kOffsetChannelData; + if(READ_BYTES_FROM(offset, dataBuffer.data(), dataBuffer.size())) + { + num_touches_ = 0; + fprintf( + stderr, + "Trill: error while reading from device %s at address %#x (%d)\n", + getNameFromDevice(device_type_).c_str(), + address, + address); + readErrorOccurred = true; + return 1; + } + parseNewData(shouldReadStatusByte); + return 0; +} + +void Trill::newData(const uint8_t* newData, size_t len, bool includesStatusByte) +{ + // we ensure dataBuffer's size is consistent with readI2C(), regardless + // of how many bytes are actually passed here. + dataBuffer.resize(getBytesToRead(includesStatusByte)); + memcpy(dataBuffer.data(), + newData, + std::min(len * sizeof(newData[0]), + sizeof(dataBuffer[0]) * dataBuffer.size())); + parseNewData(includesStatusByte); +} + +float Trill::channelIntToFloat(uint16_t in, float rawRescale) +{ + uint16_t thresh = (noiseThreshold >> transmissionRightShift); + if(in >= thresh) + in -= thresh; + return in * rawRescale; +} + +void Trill::parseNewData(bool includesStatusByte) +{ + // by the time this is called, dataBuffer will have been resized appropriately + uint8_t* src = this->dataBuffer.data(); + size_t srcSize = this->dataBuffer.size(); + if(!srcSize) + return; + if(includesStatusByte) + { + processStatusByte(src[0]); + src++; + srcSize--; + } + dataBufferIncludesStatusByte = includesStatusByte; + if(CENTROID != mode_) + { + // parse, rescale and copy data to public buffer + float rawRescale = this->rawRescale * (1 << transmissionRightShift); + switch(transmissionWidth) + { + default: + case 16: + for(unsigned int i = 0; i < getNumChannels(); ++i) + { + rawData[i] = channelIntToFloat( + (src[2 * i] << 8) + src[2 * i + 1], rawRescale); + } + break; + case 12: + { + uint8_t* p = src; + const uint8_t* end = src + srcSize; + for(unsigned int i = 0; i < getNumChannels() && p < end; ++i) + { + uint16_t val; + if(i & 1) + { + val = ((*p++) & 0xf0) << 4; + val |= *p++; + } + else + { + val = *p++ << 4; + val |= (*p & 0xf); + } + rawData[i] = channelIntToFloat(val, rawRescale); + } + } + break; + case 8: + for(unsigned int i = 0; i < getNumChannels(); ++i) + rawData[i] = channelIntToFloat(src[i], rawRescale); + break; + } + } + else + { + unsigned int locations = 0; + // Look for 1st instance of 0xFFFF (no touch) in the buffer + for(locations = 0; locations < MAX_TOUCH_1D_OR_2D; locations++) + { + if(src[2 * locations] == 0xFF && src[2 * locations + 1] == 0xFF) + break; + } + num_touches_ = locations; + + if(device_type_ == SQUARE || device_type_ == HEX) + { + // Look for the number of horizontal touches in 2D sliders + // which might be different from number of vertical touches + for(locations = 0; locations < MAX_TOUCH_1D_OR_2D; locations++) + { + if(src[2 * locations + 4 * MAX_TOUCH_1D_OR_2D] == 0xFF + && src[2 * locations + 4 * MAX_TOUCH_1D_OR_2D + 1] == 0xFF) + break; + } + num_touches_ |= (locations << 4); + } + } +} + +void Trill::processStatusByte(uint8_t newStatusByte) +{ + statusByte = newStatusByte; + uint8_t newFrameId = TrillStatusByte::parse(statusByte).frameId; + if(newFrameId < (frameId & 0x3f)) + frameId += 0x40; + frameId = (frameId & 0xffffffc0) | (newFrameId); +} + +int Trill::readStatusByte() +{ + REQUIRE_FW_AT_LEAST(3); + uint8_t newStatusByte; + if(READ_BYTE_FROM(kOffsetStatusByte, newStatusByte)) + return -1; + processStatusByte(newStatusByte); + return newStatusByte; +} + +bool Trill::hasReset() +{ + return !TrillStatusByte::parse(statusByte).initialised; +} + +bool Trill::hasActivity() +{ + return TrillStatusByte::parse(statusByte).activity; +} + +uint8_t Trill::getFrameId() +{ + return TrillStatusByte::parse(statusByte).frameId; +} + +uint32_t Trill::getFrameIdUnwrapped() +{ + return frameId; +} + +bool Trill::is1D() +{ + if(CENTROID != mode_) + return false; + switch(device_type_) + { + case BAR: + case RING: + case CRAFT: + case FLEX: return true; + default: return false; + } +} + +bool Trill::is2D() +{ + if(CENTROID != mode_) + return false; + switch(device_type_) + { + case SQUARE: + case HEX: return true; + default: return false; + } +} + +unsigned int Trill::getNumTouches() +{ + if(mode_ != CENTROID) + return 0; + + // Lower 4 bits hold number of 1-axis or vertical touches + return (num_touches_ & 0x0F); +} + +unsigned int Trill::getNumHorizontalTouches() +{ + if(mode_ != CENTROID || (device_type_ != SQUARE && device_type_ != HEX)) + return 0; + + // Upper 4 bits hold number of horizontal touches + return (num_touches_ >> 4); +} +#define dbOffset (dataBufferIncludesStatusByte * sizeof(TrillStatusByte)) + +float Trill::touchLocation(uint8_t touch_num) +{ + if(mode_ != CENTROID) + return -1; + if(touch_num >= MAX_TOUCH_1D_OR_2D) + return -1; + + int location = dataBuffer[dbOffset + 2 * touch_num] * 256; + location += dataBuffer[dbOffset + 2 * touch_num + 1]; + + return location * posRescale; +} + +float Trill::getButtonValue(uint8_t button_num) +{ + if(mode_ != CENTROID) + return -1; + if(button_num > 1) + return -1; + if(device_type_ != RING) + return -1; + + return (((dataBuffer[dbOffset + 4 * MAX_TOUCH_1D_OR_2D + 2 * button_num] + << 8) + + dataBuffer[dbOffset + 4 * MAX_TOUCH_1D_OR_2D + 2 * button_num + + 1]) + & 0x0FFF) + * rawRescale; +} + +float Trill::touchSize(uint8_t touch_num) +{ + if(mode_ != CENTROID) + return -1; + if(touch_num >= MAX_TOUCH_1D_OR_2D) + return -1; + + int size + = dataBuffer[dbOffset + 2 * touch_num + 2 * MAX_TOUCH_1D_OR_2D] * 256; + size += dataBuffer[dbOffset + 2 * touch_num + 2 * MAX_TOUCH_1D_OR_2D + 1]; + + return size * sizeRescale; +} + +float Trill::touchHorizontalLocation(uint8_t touch_num) +{ + if(mode_ != CENTROID || (device_type_ != SQUARE && device_type_ != HEX)) + return -1; + if(touch_num >= MAX_TOUCH_1D_OR_2D) + return -1; + + int location + = dataBuffer[dbOffset + 2 * touch_num + 4 * MAX_TOUCH_1D_OR_2D] * 256; + location + += dataBuffer[dbOffset + 2 * touch_num + 4 * MAX_TOUCH_1D_OR_2D + 1]; + + return location * posHRescale; +} + +float Trill::touchHorizontalSize(uint8_t touch_num) +{ + if(mode_ != CENTROID || (device_type_ != SQUARE && device_type_ != HEX)) + return -1; + if(touch_num >= MAX_TOUCH_1D_OR_2D) + return -1; + + int size + = dataBuffer[dbOffset + 2 * touch_num + 6 * MAX_TOUCH_1D_OR_2D] * 256; + size += dataBuffer[dbOffset + 2 * touch_num + 6 * MAX_TOUCH_1D_OR_2D + 1]; + + return size * sizeRescale; +} + +#define compoundTouch(LOCATION, SIZE, TOUCHES) \ + { \ + float avg = 0; \ + float totalSize = 0; \ + unsigned int numTouches = TOUCHES; \ + for(unsigned int i = 0; i < numTouches; i++) \ + { \ + avg += LOCATION(i) * SIZE(i); \ + totalSize += SIZE(i); \ + } \ + if(numTouches) \ + avg = avg / totalSize; \ + return avg; \ + } + +float Trill::compoundTouchLocation() +{ + compoundTouch(touchLocation, touchSize, getNumTouches()); +} + +float Trill::compoundTouchHorizontalLocation() +{ + compoundTouch(touchHorizontalLocation, + touchHorizontalSize, + getNumHorizontalTouches()); +} + +float Trill::compoundTouchSize() +{ + float size = 0; + for(unsigned int i = 0; i < getNumTouches(); i++) + size += touchSize(i); + return size; +} + +unsigned int Trill::getNumChannels() const +{ + return numChannels; +} + +unsigned int Trill::getDefaultNumChannels() const +{ + switch(device_type_) + { + case BAR: return kNumChannelsBar; + case RING: return kNumChannelsRing; + default: return kNumChannelsMax; + } +} diff --git a/src/dev/trill/Trill.h b/src/dev/trill/Trill.h new file mode 100644 index 000000000..70c802e8a --- /dev/null +++ b/src/dev/trill/Trill.h @@ -0,0 +1,637 @@ +#pragma once +#include "I2c.h" +#include +#include +#include + +/** + * \brief A class to use the Trill family of capacitive sensors. + * http://bela.io/trill + * \nosubgrouping + */ + +class Trill : public I2c +{ + public: + /** + * The acquisition modes that a device can be set to. + */ + typedef enum + { + AUTO = -1, /**< Auto mode: the mode is set + automatically based on the device type */ + CENTROID = 0, /**< Centroid mode: detect discrete touches */ + RAW = 1, /**< Raw mode */ + BASELINE = 2, /**< Baseline mode */ + DIFF = 3, /**< Differential mode */ + } Mode; + + /** + * The types of Trill devices + */ + typedef enum + { + NONE = -1, ///< No device + UNKNOWN = 0, ///< A valid device of unknown type + BAR = 1, ///< %Trill Bar + SQUARE = 2, ///< %Trill Square + CRAFT = 3, ///< %Trill Craft + RING = 4, ///< %Trill Ring + HEX = 5, ///< %Trill Hex + FLEX = 6, ///< %Trill Flex + } Device; + /** + * Controls when the EVT pin will be set when a new frame is + * available. In this context, the meaning "activity is detected" + * depends on the #Mode in which the device is: + * - in #CENTROID mode, activity is detected if one or more + * touches are detected + * - in any other mode, activity is detected if any channel + * after formatting is non-zero. + */ + typedef enum + { + kEventModeTouch + = 0, ///< Only set the EVT pin if activity is detected in the current frame + kEventModeChange + = 1, ///< Only set the EVT pin if activity is detected in the current or past frame + kEventModeAlways + = 2, ///< Set the EVT pin every time a new frame is available + } EventMode; + /** + * + */ + typedef enum + { + kScanTriggerDisabled = 0x0, ///< Do not scan capacitive channels. + kScanTriggerI2c + = 0x1, ///< Scan capacitive channels after every I2C transaction + kScanTriggerTimer + = 0x2, ///< Scan capacitive channels every time the timer set by setAutoScanInterval() expires + kScanTriggerI2cOrTimer + = 0x3, ///< Scan capacitive channels after every I2C transaction or when timer expires, whichever comes first. + } ScanTriggerMode; + + private: + Mode mode_; // Which mode the device is in + Device device_type_ = NONE; // Which type of device is connected (if any) + uint32_t frameId; + uint8_t statusByte; + uint8_t address; + uint8_t firmware_version_ = 0; // Firmware version running on the device + uint8_t num_touches_; // Number of touches on last read + bool dataBufferIncludesStatusByte = false; + std::vector dataBuffer; + uint16_t commandSleepTime = 1000; + size_t currentReadOffset = -1; + unsigned int numBits; + unsigned int transmissionWidth = 16; + unsigned int transmissionRightShift = 0; + uint32_t channelMask; + uint8_t numChannels; + float posRescale; + float posHRescale; + float sizeRescale; + float rawRescale; + ScanTriggerMode scanTriggerMode; + int identify(); + void updateRescale(); + void parseNewData(bool includesStatusByte); + void processStatusByte(uint8_t newStatusByte); + int writeCommandAndHandle(const i2c_char_t* data, + size_t size, + const char* name); + int writeCommandAndHandle(i2c_char_t command, const char* name); + int readBytesFrom(uint8_t offset, + i2c_char_t* data, + size_t size, + const char* name); + int readBytesFrom(uint8_t offset, i2c_char_t& byte, const char* name); + int waitForAck(uint8_t command, const char* name); + void updateChannelMask(uint32_t mask); + float channelIntToFloat(uint16_t in, float rawRescale); + uint8_t noiseThreshold = 0; + int verbose = 0; + uint8_t cmdCounter = 0; + bool readErrorOccurred; + bool enableVersionCheck = true; + + public: + /** + * @name RAW, BASELINE or DIFF mode + * When the device is in #RAW, #BASELINE, or #DIFF mode, the + * readings from the individual sensing channels are accessed + * through #rawData. + * @{ + * @class TAGS_canonical_return + * @return 0 on success or an error code otherwise. + * + * @class TAGS_firmware_3_error + * \note This feature is only available with devices starting + * from firmware version 3. On older devices calling this + * function has no effect and it will return an error. + * + * @class TAGS_firmware_3_undef + * \note This feature is only available with devices starting from firmware + * version 3. On older devices calling this function has no effect and its + * return value is undefined. + */ + /** + * An array containing the readings from the device's + * channel when the device is in + * #RAW, #BASELINE or #DIFF mode. + * + * The type of data it contains depend on the device mode: + * - #RAW: the #rawData array contains + * the raw readings of each individual capacitive sensing + * channel. This corresponds to `CSD_waSnsResult`. + * - #BASELINE:the #rawData + * array contains the baseline readings of each individual + * capacitive sensing channel. + * This corresponds to `CSD_waSnsBaseline`. + * - #DIFF: the #rawData array + * contains differential readings between the baseline and + * the raw reading. This corresponds to `CSD_waSnsDiff`. + */ + std::vector rawData; + /** @} */ + + /** + * An array containing the valid values for the speed parameter + * in setScanSettings() + */ + static constexpr uint8_t speedValues[4] = {0, 1, 2, 3}; + /** + * The maximum value for the setPrescaler() method + */ + static constexpr uint8_t prescalerMax = 8; + Trill(); + ~Trill(); + /** + * Initialise the device. + * + * @param i2c_bus the bus that the device is connected to. + * @param device the device type. If #UNKNOWN is passed, then + * the \p i2c_address parameter has to be a valid address, and + * any detected device type will be accepted. If something else + * than #UNKNOWN is passed, and the detected device type is + * different from the requested one, the function will fail and + * the object will be left uninitialised. + * @param i2c_address the address at which the device can be + * found. If `255` or no value is passed, the default address + * for the specified device type will be used. + */ + Trill(unsigned int i2c_bus, Device device, uint8_t i2c_address = 255); + /** + * \copydoc Trill::Trill(unsigned int, Device, uint8_t) + * + * \copydoc TAGS_canonical_return + */ + int setup(unsigned int i2c_bus, Device device, uint8_t i2c_address = 255); + + /** + * Probe the bus for a device at the specified address. + * + * @return The type of the device that was found. If no device + * was found, #NONE is returned. + */ + static Device probe(unsigned int i2c_bus, uint8_t i2c_address); + + /** + * Update the baseline value on the device. + */ + int updateBaseline(); + /** + * Reset the chip. + */ + int reset(); + + /** + * \brief Read data from the device. + * + * Performs an I2C transaction with the device to retrieve new data + * and parse them. Users calling this method won't need to call newData(). + * + * @param shouldReadStatusByte whether or not to read the + * status byte as part of the transaction. If the firmware + * version is lower than 3, this should be set to `false`. + * + * \copydoc TAGS_canonical_return + */ + int readI2C(bool shouldReadStatusByte = false); + + /** + * \brief Set data retrieved from the device. + * + * Sets the data retrieved from the device. + * This can be used to pass to the object + * data retrieved elsewhere (e.g.: from an I2C DMA callback). + * Users calling readI2C() won't need to call this method. + * + * @param newData A pointer to an array containing new data. + * @param len The length of the array. For proper operation, this + * should be the value returned from getBytesToRead(). + * @param includesStatusByte whether #newData includes the + * status byte or not. + */ + void newData(const uint8_t* newData, + size_t len, + bool includesStatusByte = false); + + /** + * Get the device type. + */ + Device deviceType() { return device_type_; } + /** + * Get the name from the device. + */ + static const std::string& getNameFromDevice(Device device); + /** + * Get the device from the name. + */ + static Device getDeviceFromName(const std::string& name); + /** + * Get the mode from the name. + */ + static const std::string& getNameFromMode(Mode mode); + /** + * Get the mode from the name. + */ + static Mode getModeFromName(const std::string& name); + /** + * Get the firmware version of the device. + */ + int firmwareVersion() { return firmware_version_; } + /** + * Get the mode that the device is currently in. + */ + Mode getMode() { return mode_; } + /** + * Get the current address of the device. + */ + uint8_t getAddress() { return address; } + /** + * Print details about the device to standard output + */ + void printDetails(); + /** + * Print more details about I/O transactions as they happen. + */ + void setVerbose(int verbose); + /** + * Get the number of capacitive channels currently active on the device. + */ + unsigned int getNumChannels() const; + /** + * Get the number of capacitive channels available on the device. + */ + unsigned int getDefaultNumChannels() const; + + /** + * @name Scan Configuration Settings + * @{ + * + * Some of the methods below map directly to function calls and + * variables with the `CSD_` prefix, which are described in the + * [Cypress CapSense Sigma-Delta * Datasheet + * v2.20](https://www.cypress.com/file/124551/download). + */ + /** + * Set the operational mode of the device. + * + * @param mode The device mode. The special mode #AUTO, selects the + * device-specific default mode for the _detected_ device type. + * \copydoc TAGS_canonical_return + */ + int setMode(Mode mode); + /** + * Set the speed and bit depth of the capacitive scanning. + * This triggers a call to `CSD_SetScanMode(speed, num_bits)` + * on the device. + * + * @param speed The speed of the scanning + * Valid values of speed are, ordered by decreasing speed, are + * comprised between 0 (`CSD_ULTRA_FAST_SPEED`) and 3 (`CSD_SLOW_SPEED`) + * @param num_bits The bit depth of the scanning. + * Valid values are comprised between 9 and 16. + * \copydoc TAGS_canonical_return + */ + int setScanSettings(uint8_t speed, uint8_t num_bits = 12); + /** + * Set the prescaler value for the capacitive scanning. + * This triggers a call to `CSD_SetPrescaler(prescaler)` + * on the device. + * + * @param prescaler The prescaler value. Valid values are + * between 0 and 8, inclusive, and map directly to values + * `CSD_PRESCALER_1` to `CSD_PRESCALER_256`. + * \copydoc TAGS_canonical_return + */ + int setPrescaler(uint8_t prescaler); + /** + * Set the noise threshold for the capacitive channels. + * + * When a channel's scan returns a value smaller than the + * threshold, its value is set to 0. + * + * @param threshold the noise threshold level. Valid values are + * between 0 and `255.0/(1 << numBits)`. + * The value is internally converted to an 8-bit integer by + * multiplying it times `1 << numBits` before being sent to the device. + * On the device, the received value is used to set the + * `CSD_bNoiseThreshold` variable. + * @return 0 on success, or an error code otherwise. + */ + int setNoiseThreshold(float threshold); + /** + * Sets the IDAC value for the device. + * + * This triggers a call to `CSD_SetIdacValue(value)` on the device. + * + * @param value the IDAC value. Valid values are between 0 and 255. + * \copydoc TAGS_canonical_return + */ + int setIDACValue(uint8_t value); + /** + * Set minimum touch size + * + * Sets the minimum touch size below which a touch is ignored. + * \copydoc TAGS_canonical_return + * + */ + int setMinimumTouchSize(float minSize); + /** + * Set how the device triggers a new scan of its capacitive + * channels. + * + * @param arg One of the #ScanTriggerMode values + * + * \copydoc TAGS_firmware_3_error + * \copydoc TAGS_canonical_return + */ + int setScanTrigger(ScanTriggerMode scanTriggerMode); + /** + * Set the interval for scanning capacitive channels when the + * device's scanning is triggered by the timer. + * + * @param ms the scanning period, measured in milliseconds. The + * effective minimum scanning period will be limited by the scanning + * speed, bit depth and any computation happening on the device + * (such as touch detection). Granularity is 1 ms for values + * until 255 ms and higher after that. Maximum value is just + * above 2032 ms. + * Scanning on timer has to be separately enabled via setScanTrigger(). + * When @p ms is not greater than zero, the timer is disabled. + * + * \note The 32kHz clock often deviates by 10% or more from its + * nominal frequency, thus affecting the accuracy of the timer. + */ + int setTimerPeriod(float ms); + /** + * Deprecated. Same as setTimerPeriod(), but the @p interval is + * expressed as cycles of a 32kHz clock. On devices with + * firmware 2, @p interval is used directly. On devices with + * firwmare 3 or above, it is quantised to blocks of at least + * 1 ms. + */ + int setAutoScanInterval(uint16_t interval); + /** + * Set how the EVT pin behaves. + * + * @param mode an #EventMode denoting the required behaviour. + * + * \copydoc TAGS_canonical_return + * \copydoc TAGS_firmware_3_error + */ + int setEventMode(EventMode mode); + /** + * Set a channel mask identifying which scanning channels are + * enabled. + * + * @param mask The channel mask. Bits 0 to 31 identify channels + * 0 to 31 respectively. Bit positions higher than the value + * returned by getDefaultNumChannels() are ignored. + * + * \copydoc TAGS_canonical_return + * \copydoc TAGS_firmware_3_error + */ + int setChannelMask(uint32_t mask); + /** + * Set the format used for transmission of non-centroid data + * from the device to the host. + * + * @param width The data width. If a value would overflow when + * stored, it is clipped. + * @param shift Number of right shift operations applied on the + * value before being stored in the word. + * + * \copydoc TAGS_canonical_return + * \copydoc TAGS_firmware_3_error + */ + int setTransmissionFormat(uint8_t width, uint8_t shift); + /** @} */ // end of Scan Configuration Settings + /** + * @name Status byte + * @{ + */ + /** + * Read the status byte from the device. + * Alternatively, the status byte can be read as part of + * reading data by calling readI2C(true). + * + * @return the status byte, or a negative value in case of + * error. As a successful call also updates the + * internal state, the caller is probably better off calling + * getFrameId(), hasActivity(), hasReset() instead of parsing + * the status byte directly. + * + * \copydoc TAGS_firmware_3_undef + */ + int readStatusByte(); + /** + * Whether the device has reset since a identify command was + * last written to it. + * + * This relies on a current status byte. + * + * \copydoc TAGS_firmware_3_error + */ + bool hasReset(); + /** + * Whether activity has been detected in the current frame. + * + * This relies on a current status byte. + * + * \copydoc TAGS_firmware_3_undef + */ + bool hasActivity(); + /** + * Get the frameId. + * This relies on a current status byte. + * + * \copydoc TAGS_firmware_3_undef + */ + uint8_t getFrameId(); + /** + * Same as above, but it tries to unwrap the 6-bit frameId into + * a uint32_t counter. + * This relies on reading several status bytes over time. + * The counter is guaranteed monotonic, but it can only be + * regarded as an actual frame counter if the status byte is + * read at least once every 63 frames. + * + * @return the counter + * \copydoc TAGS_firmware_3_undef + */ + uint32_t getFrameIdUnwrapped(); + /** + * @} + */ + + /** + * @name Centroid Mode + * @{ + * + * When the device is in #CENTROID mode, touches are + * detected as discrete entities and can be retrieved with + * the methods in this section. + * + * The `location` of a touch is a normalised value where `0` and + * `1` are the extremes of the axis. + * + * The `size` of a touch is a rescaled value of the total + * activation measured on the sensing channels that contribute + * to the touch. The amount of activation for a touch of a + * given size is dependent (among other things) on the geometry + * of the device. The values used here have been determined + * empirically. + * + * A `compoundTouch` is a single touch represntation obtained + * by averaging the location and size of the touches on each + * axis and their size. + * This is most useful for 2-axes devices, in order to get a + * single touch. + * + * @class TAGS_1d + * \note It is only valid to call this method if one of is1D() and + * is2D() returns `true`. + * @class TAGS_2d + * \note It is only valid to call this method is2D() returns `true` + */ + /** + * Does the device have one axis of position sensing? + * + * @return `true` if the device has one axis of position sensing + * and is set in #CENTROID mode, `false` + * otherwise. + */ + bool is1D(); + /** + * Does the device have two axes of position sensing? + * + * @return `true` if the device has two axes of position sensing + * and is set in #CENTROID mode, `false` + * otherwise. + */ + bool is2D(); + /** + * Return the number of bytes to read when reading data. + */ + unsigned int getBytesToRead(bool includesStatusByte); + /** + * Return the number of "button" channels on the device. + */ + unsigned int getNumButtons() + { + return 2 * (getMode() == CENTROID && RING == deviceType()); + }; + /** + * Get the number of touches currently active on the + * vertical axis of the device. + * + * \copydoc TAGS_1d + */ + unsigned int getNumTouches(); + /** + * Get the location of a touch on the vertical axis of the + * device. + * + * \copydoc TAGS_1d + * + * @param touch_num the number of the touch. This value needs + * to be comprised between 0 and `getNumTouches() - 1`. + * @return the position of the touch relative to the axis, or + * -1 if no such touch exists. + */ + float touchLocation(uint8_t touch_num); + /** + * Get the size of a touch. + * + * \copydoc TAGS_1d + * + * @return the size of the touch, if the touch exists, or 0 + * otherwise. + */ + float touchSize(uint8_t touch_num); + /** + * Get the number of touches currently active on the + * horizontal axis of the device. + * + * \copydoc TAGS_2d + */ + unsigned int getNumHorizontalTouches(); + /** + * Get the location of a touch on the horizontal axis of the + * device. + * + * \copydoc TAGS_2d + * + * @param touch_num the number of the touch. This value needs + * to be comprised between 0 and `getNumHorizontalTouches() - 1`. + * @return the position of the touch relative to the axis, or + * -1 if no such touch exists. + * */ + float touchHorizontalLocation(uint8_t touch_num); + /** + * Get the size of a touch. + * + * \copydoc TAGS_2d + * + * @return the size of the touch, if the touch exists, or 0 + * otherwise. + */ + float touchHorizontalSize(uint8_t touch_num); + /** + * Get the vertical location of the compound touch on the + * device. + * + * \copydoc TAGS_1d + * */ + float compoundTouchLocation(); + /** + * Get the horizontal location of the compound touch on the + * device. + * + * \copydoc TAGS_1d + */ + float compoundTouchHorizontalLocation(); + /** + * Get the size of the compound touch on the + * device. + * + * \copydoc TAGS_1d + */ + float compoundTouchSize(); + /** + * Get the value of the capacitive "button" channels on the + * device + * + * @param button_num the button number. Valid values are + * comprised between `0` and `getNumButtons() - 1`. + * @return The differential reading on the button, normalised + * between 0 and 1. + */ + float getButtonValue(uint8_t button_num); + + /** @}*/ // end of centroid mode +}; diff --git a/src/dev/trill/calculateCentroids.h b/src/dev/trill/calculateCentroids.h new file mode 100644 index 000000000..6e8d7bc50 --- /dev/null +++ b/src/dev/trill/calculateCentroids.h @@ -0,0 +1,152 @@ +// returns a WORD packing two signed chars. The high bytes is the last active sensor in the last centroid, +// while the low byte is the first active sensor of the last centroid +WORD calculateCentroids(WORD *centroidBuffer, + WORD *sizeBuffer, + BYTE maxNumCentroids, + BYTE minSensor, + BYTE maxSensor, + BYTE numSensors) +{ + BYTE lastActiveSensor = 255; + BYTE centroidIndex = 0, sensorIndex, actualHardwareIndex; + BYTE wrappedAround = 0; + BYTE inCentroid = 0; + WORD peakValue = 0, troughDepth = 0; + long temp; + + WORD lastSensorVal, currentSensorVal, currentWeightedSum, + currentUnweightedSum; + BYTE currentStart, currentLength; + + for(sensorIndex = 0; sensorIndex < maxNumCentroids; sensorIndex++) + { + centroidBuffer[sensorIndex] = 0xFFFF; + sizeBuffer[sensorIndex] = 0; + } + + currentSensorVal = 0; + + for(sensorIndex = 0, actualHardwareIndex = minSensor; + sensorIndex < numSensors; + sensorIndex++) + { + lastSensorVal = currentSensorVal; + + currentSensorVal = CSD_waSnsDiff[actualHardwareIndex++]; + if(currentSensorVal > 0) + { + lastActiveSensor = sensorIndex; + } + // if we get to the end, and there is more to go, wrap around + if(actualHardwareIndex == maxSensor) + { + actualHardwareIndex = minSensor; + // once we wrap around, if we find ourselves out of a centroid, + // any centroids detected after the then current point onwards + // would be equal or worse than the ones we already got earlier for + // the same sensors, so we will have to break + wrappedAround = 1; + } + + if(inCentroid) + { + // Currently in the middle of a group of sensors constituting a centroid. Use a zero sample + // or a spike above a certain magnitude to indicate the end of the centroid. + + if(currentSensorVal == 0) + { + if(currentUnweightedSum > wMinimumCentroidSize) + { + temp = ((long)currentWeightedSum << SLIDER_BITS) + / currentUnweightedSum; + centroidBuffer[centroidIndex] + = (currentStart << SLIDER_BITS) + (WORD)temp; + sizeBuffer[centroidIndex] = currentUnweightedSum; + centroidIndex++; + } + + inCentroid = 0; + if(wrappedAround) + { + break; + } + if(centroidIndex >= maxNumCentroids) + break; + continue; + } + + if(currentSensorVal > peakValue) // Keep tabs on max and min values + peakValue = currentSensorVal; + if(peakValue - currentSensorVal > troughDepth) + troughDepth = peakValue - currentSensorVal; + + // If this sensor value is a significant increase over the last one, AND the last one was decreasing, then start a new centroid. + // In other words, identify a trough in the values and use it to segment into two centroids. + + if(sensorIndex >= 2) + { + if(troughDepth > wAdjacentCentroidNoiseThreshold + && currentSensorVal + > lastSensorVal + wAdjacentCentroidNoiseThreshold) + { + if(currentUnweightedSum > wMinimumCentroidSize) + { + temp = ((long)currentWeightedSum << SLIDER_BITS) + / currentUnweightedSum; + centroidBuffer[centroidIndex] + = (currentStart << SLIDER_BITS) + (WORD)temp; + sizeBuffer[centroidIndex] = currentUnweightedSum; + centroidIndex++; + } + inCentroid = 0; + if(wrappedAround) + { + break; + } + if(centroidIndex >= maxNumCentroids) + break; + inCentroid = 1; + currentStart = sensorIndex; + currentUnweightedSum = peakValue = currentSensorVal; + currentLength = 1; + currentWeightedSum = 0; + troughDepth = 0; + continue; + } + } + + currentUnweightedSum += currentSensorVal; + currentWeightedSum += currentLength * currentSensorVal; + currentLength++; + } + else + { + // Currently not in a centroid (zeros between centroids). Look for a new sample to initiate centroid. + if(currentSensorVal > 0) + { + currentStart = sensorIndex; + currentUnweightedSum = peakValue = currentSensorVal; + currentLength = 1; + currentWeightedSum = 0; + troughDepth = 0; + inCentroid = 1; + } + } + if(!inCentroid && wrappedAround) + { + break; + } + } + + // Finish up the calculation on the last centroid, if necessary + if(inCentroid && currentUnweightedSum > wMinimumCentroidSize) + { + temp = ((long)currentWeightedSum << SLIDER_BITS) / currentUnweightedSum; + centroidBuffer[centroidIndex] + = (currentStart << SLIDER_BITS) + (WORD)temp; + sizeBuffer[centroidIndex] = currentUnweightedSum; + centroidIndex++; + } + + return (lastActiveSensor << 8) | currentStart; +}