diff --git a/examples/Trill-Craft/Trill-Craft.cpp b/examples/Trill-Craft/Trill-Craft.cpp index eae584e4d..5369dbf71 100644 --- a/examples/Trill-Craft/Trill-Craft.cpp +++ b/examples/Trill-Craft/Trill-Craft.cpp @@ -22,7 +22,7 @@ int main(void) // 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); + int ret = trill.setup(i2cBus, Trill::CRAFT); if(ret) hw.PrintLine("trill.setup() returned %d", ret); @@ -32,10 +32,10 @@ int main(void) hw.DelayMs(100); trill.readI2C(); for(auto &x : trill.rawData) - { - hw.Print("%d ", int(x*100000.f)); + { + hw.Print("%d ", int(x * 100000.f)); } hw.PrintLine(""); } - return 0; + return 0; } diff --git a/examples/Trill/Trill.cpp b/examples/Trill/Trill.cpp index 1277f4369..1d54bb77f 100644 --- a/examples/Trill/Trill.cpp +++ b/examples/Trill/Trill.cpp @@ -10,9 +10,9 @@ using namespace daisy::seed; DaisySeed hw; -void AudioCallback(AudioHandle::InputBuffer in, +void AudioCallback(AudioHandle::InputBuffer in, AudioHandle::OutputBuffer out, - size_t size) + size_t size) { // } @@ -33,7 +33,7 @@ int main(void) // 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); + int ret = trill.setup(i2cBus, Trill::BAR); if(ret) hw.Print("trill.setup() returned %d\n", ret); @@ -44,7 +44,8 @@ int main(void) if(trill.getNumTouches()) { for(size_t n = 0; n < trill.getNumTouches(); ++n) - hw.Print("%.3f (%.2f) ", trill.touchLocation(n), trill.touchSize(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 index 3ca011f30..e43df1372 100644 --- a/src/dev/trill/CentroidDetection.cpp +++ b/src/dev/trill/CentroidDetection.cpp @@ -4,219 +4,258 @@ // 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: + 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; - } - } - } + 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) +CentroidDetection::CentroidDetection(unsigned int numReadings, + unsigned int maxNumCentroids, + float sizeScale) { - setup(numReadings, maxNumCentroids, sizeScale); + setup(numReadings, maxNumCentroids, sizeScale); } -CentroidDetection::CentroidDetection(const std::vector& order, unsigned int maxNumCentroids, float sizeScale) +CentroidDetection::CentroidDetection(const std::vector& order, + unsigned int maxNumCentroids, + float sizeScale) { - setup(order, maxNumCentroids, sizeScale); + setup(order, maxNumCentroids, sizeScale); } -int CentroidDetection::setup(unsigned int numReadings, unsigned int maxNumCentroids, float 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); + 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) +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; + 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; + 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)); + 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; + 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; + this->sizeScale = 1.f / sizeScale; } void CentroidDetection::setMinimumTouchSize(DATA_T minSize) { - cc->wMinimumCentroidSize = minSize; + cc->wMinimumCentroidSize = minSize; } void CentroidDetection::setNoiseThreshold(DATA_T threshold) { - noiseThreshold = threshold; + noiseThreshold = threshold; } unsigned int CentroidDetection::getNumTouches() const { - return num_touches; + return num_touches; } -CentroidDetection::DATA_T CentroidDetection::touchLocation(unsigned int touch_num) const +CentroidDetection::DATA_T +CentroidDetection::touchLocation(unsigned int touch_num) const { - if(touch_num < maxNumCentroids) - return centroids[touch_num]; - else - return 0; + if(touch_num < maxNumCentroids) + return centroids[touch_num]; + else + return 0; } -CentroidDetection::DATA_T CentroidDetection::touchSize(unsigned int touch_num) const +CentroidDetection::DATA_T +CentroidDetection::touchSize(unsigned int touch_num) const { - if(touch_num < num_touches) - return sizes[touch_num]; - else - return 0; + 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;\ - } +#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()); + 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; + 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; + this->min = min; + this->max = max; } -static inline float map(float x, float in_min, float in_max, float out_min, float out_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; + 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; + 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) +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; + 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); - } + 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 index 6677f3027..1ccba9ad7 100644 --- a/src/dev/trill/CentroidDetection.h +++ b/src/dev/trill/CentroidDetection.h @@ -5,60 +5,72 @@ 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); - /** + 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); - /** + 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; + 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; + 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 index e593d3dfe..8f491d736 100644 --- a/src/dev/trill/I2c.h +++ b/src/dev/trill/I2c.h @@ -8,79 +8,86 @@ 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; -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(); + public: + I2c(){}; + I2c(I2c &&) = delete; + int initI2C_RW(int bus, int address, int dummy); + int closeI2C(); - virtual ~I2c(); - daisy::I2CHandle i2cHandle; + 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; + 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; + 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; + 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; + if(daisy::I2CHandle::Result::OK + == i2cHandle.TransmitBlocking( + i2cAddress, (uint8_t *)buf, count, kTimeout)) + return count; + else + return -1; } -inline I2c::~I2c(){} +inline I2c::~I2c() {} #include "sys/system.h" inline int usleep(useconds_t us) { - daisy::System::Delay((us + 999) / 1000); - return 0; + daisy::System::Delay((us + 999) / 1000); + return 0; } diff --git a/src/dev/trill/Trill.cpp b/src/dev/trill/Trill.cpp index acfc36405..6e7196c02 100644 --- a/src/dev/trill/Trill.cpp +++ b/src/dev/trill/Trill.cpp @@ -5,972 +5,1127 @@ #ifndef __try #if __cpp_exceptions -# define __try try -# define __catch(X) catch(X) -# define __throw_exception_again throw +#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 +#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)) +#define MAX_TOUCH_1D_OR_2D \ + (((device_type_ == SQUARE || device_type_ == HEX) ? kMaxTouchNum2D \ + : kMaxTouchNum1D)) -enum { - kCentroidLengthDefault = 20, - kCentroidLengthRing = 24, - kCentroidLength2D = 32, - kRawLength = 60, +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 +{ + 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 +{ + kOffsetCommand = 0, + kOffsetStatusByte = 3, + kOffsetChannelData = 4, }; -enum { - kNumChannelsBar = 26, - kNumChannelsRing = 30, - kNumChannelsMax = 30, +enum +{ + kNumChannelsBar = 26, + kNumChannelsRing = 30, + kNumChannelsMax = 30, }; -enum { - kMaxTouchNum1D = 5, - kMaxTouchNum2D = 4 +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; + 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)}, + {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"}, + {Trill::AUTO, "Auto"}, + {Trill::CENTROID, "Centroid"}, + {Trill::RAW, "Raw"}, + {Trill::BASELINE, "Baseline"}, + {Trill::DIFF, "Diff"}, }; -struct trillRescaleFactors_t { - float pos; - float posH; - float size; +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, +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); - } +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"); +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() {} -Trill::Trill(unsigned int i2c_bus, Device device, uint8_t i2c_address) { - setup(i2c_bus, device, i2c_address); +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)); + 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; + 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 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(); +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; - } + __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; + 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; + 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); - } + __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; + 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); +#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; + 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; + 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); + 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()); + 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; + 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; + 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::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::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; + 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; + 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::updateBaseline() +{ + return WRITE_COMMAND(kCommandBaselineUpdate); } -int Trill::reset() { - REQUIRE_FW_AT_LEAST(3); - return WRITE_COMMAND(kCommandReset); +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; - } + 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; + 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); + // 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; + 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); - } - } + // 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); + 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; + 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; + return !TrillStatusByte::parse(statusByte).initialised; } bool Trill::hasActivity() { - return TrillStatusByte::parse(statusByte).activity; + return TrillStatusByte::parse(statusByte).activity; } -uint8_t Trill::getFrameId() { - return TrillStatusByte::parse(statusByte).frameId; +uint8_t Trill::getFrameId() +{ + return TrillStatusByte::parse(statusByte).frameId; } -uint32_t Trill::getFrameIdUnwrapped() { - return 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; - } + 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; - } + 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; + if(mode_ != CENTROID) + return 0; - // Lower 4 bits hold number of 1-axis or vertical touches - return (num_touches_ & 0x0F); + // 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; + if(mode_ != CENTROID || (device_type_ != SQUARE && device_type_ != HEX)) + return 0; - // Upper 4 bits hold number of horizontal touches - return (num_touches_ >> 4); + // 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; + 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]; + int location = dataBuffer[dbOffset + 2 * touch_num] * 256; + location += dataBuffer[dbOffset + 2 * touch_num + 1]; - return location * posRescale; + 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; + 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; + 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]; + 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; + 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; + 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]; + 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; + 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; + 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]; + 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; + 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;\ - } +#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()); + compoundTouch(touchLocation, touchSize, getNumTouches()); } float Trill::compoundTouchHorizontalLocation() { - compoundTouch(touchHorizontalLocation, touchHorizontalSize, getNumHorizontalTouches()); + compoundTouch(touchHorizontalLocation, + touchHorizontalSize, + getNumHorizontalTouches()); } float Trill::compoundTouchSize() { - float size = 0; - for(unsigned int i = 0; i < getNumTouches(); i++) - size += touchSize(i); - return size; + float size = 0; + for(unsigned int i = 0; i < getNumTouches(); i++) + size += touchSize(i); + return size; } unsigned int Trill::getNumChannels() const { - return numChannels; + return numChannels; } unsigned int Trill::getDefaultNumChannels() const { - switch(device_type_) { - case BAR: return kNumChannelsBar; - case RING: return kNumChannelsRing; - default: return kNumChannelsMax; - } + 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 index 027145df2..70c802e8a 100644 --- a/src/dev/trill/Trill.h +++ b/src/dev/trill/Trill.h @@ -12,33 +12,35 @@ class Trill : public I2c { - public: - /** + public: + /** * The acquisition modes that a device can be set to. */ - typedef enum { - AUTO = -1, /**< Auto mode: the mode is set + 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; + 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; - /** + 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: @@ -47,60 +49,75 @@ class Trill : public I2c * - 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: - /** + 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 @@ -119,7 +136,7 @@ class Trill : public I2c * 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. @@ -136,21 +153,21 @@ class Trill : public I2c * contains differential readings between the baseline and * the raw reading. This corresponds to `CSD_waSnsDiff`. */ - std::vector rawData; - /** @} */ + 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}; - /** + 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(); - /** + static constexpr uint8_t prescalerMax = 8; + Trill(); + ~Trill(); + /** * Initialise the device. * * @param i2c_bus the bus that the device is connected to. @@ -164,32 +181,32 @@ class Trill : public I2c * 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); - /** + 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); + 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); + static Device probe(unsigned int i2c_bus, uint8_t i2c_address); - /** + /** * Update the baseline value on the device. */ - int updateBaseline(); - /** + int updateBaseline(); + /** * Reset the chip. */ - int reset(); + int reset(); - /** + /** * \brief Read data from the device. * * Performs an I2C transaction with the device to retrieve new data @@ -201,9 +218,9 @@ class Trill : public I2c * * \copydoc TAGS_canonical_return */ - int readI2C(bool shouldReadStatusByte = false); + int readI2C(bool shouldReadStatusByte = false); - /** + /** * \brief Set data retrieved from the device. * * Sets the data retrieved from the device. @@ -217,58 +234,60 @@ class Trill : public I2c * @param includesStatusByte whether #newData includes the * status byte or not. */ - void newData(const uint8_t* newData, size_t len, bool includesStatusByte = false); + void newData(const uint8_t* newData, + size_t len, + bool includesStatusByte = false); - /** + /** * Get the device type. */ - Device deviceType() { return device_type_; } - /** + Device deviceType() { return device_type_; } + /** * Get the name from the device. */ - static const std::string& getNameFromDevice(Device device); - /** + static const std::string& getNameFromDevice(Device device); + /** * Get the device from the name. */ - static Device getDeviceFromName(const std::string& name); - /** + static Device getDeviceFromName(const std::string& name); + /** * Get the mode from the name. */ - static const std::string& getNameFromMode(Mode mode); - /** + static const std::string& getNameFromMode(Mode mode); + /** * Get the mode from the name. */ - static Mode getModeFromName(const std::string& name); - /** + static Mode getModeFromName(const std::string& name); + /** * Get the firmware version of the device. */ - int firmwareVersion() { return firmware_version_; } - /** + int firmwareVersion() { return firmware_version_; } + /** * Get the mode that the device is currently in. */ - Mode getMode() { return mode_; } - /** + Mode getMode() { return mode_; } + /** * Get the current address of the device. */ - uint8_t getAddress() { return address; } - /** + uint8_t getAddress() { return address; } + /** * Print details about the device to standard output */ - void printDetails() ; - /** + void printDetails(); + /** * Print more details about I/O transactions as they happen. */ - void setVerbose(int verbose); - /** + void setVerbose(int verbose); + /** * Get the number of capacitive channels currently active on the device. */ - unsigned int getNumChannels() const; - /** + unsigned int getNumChannels() const; + /** * Get the number of capacitive channels available on the device. */ - unsigned int getDefaultNumChannels() const; + unsigned int getDefaultNumChannels() const; - /** + /** * @name Scan Configuration Settings * @{ * @@ -277,15 +296,15 @@ class Trill : public I2c * [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); - /** + 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. @@ -297,8 +316,8 @@ class Trill : public I2c * Valid values are comprised between 9 and 16. * \copydoc TAGS_canonical_return */ - int setScanSettings(uint8_t speed, uint8_t num_bits = 12); - /** + 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. @@ -308,8 +327,8 @@ class Trill : public I2c * `CSD_PRESCALER_1` to `CSD_PRESCALER_256`. * \copydoc TAGS_canonical_return */ - int setPrescaler(uint8_t prescaler); - /** + int setPrescaler(uint8_t prescaler); + /** * Set the noise threshold for the capacitive channels. * * When a channel's scan returns a value smaller than the @@ -323,8 +342,8 @@ class Trill : public I2c * `CSD_bNoiseThreshold` variable. * @return 0 on success, or an error code otherwise. */ - int setNoiseThreshold(float threshold); - /** + int setNoiseThreshold(float threshold); + /** * Sets the IDAC value for the device. * * This triggers a call to `CSD_SetIdacValue(value)` on the device. @@ -332,16 +351,16 @@ class Trill : public I2c * @param value the IDAC value. Valid values are between 0 and 255. * \copydoc TAGS_canonical_return */ - int setIDACValue(uint8_t value); - /** + 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); - /** + int setMinimumTouchSize(float minSize); + /** * Set how the device triggers a new scan of its capacitive * channels. * @@ -350,8 +369,8 @@ class Trill : public I2c * \copydoc TAGS_firmware_3_error * \copydoc TAGS_canonical_return */ - int setScanTrigger(ScanTriggerMode scanTriggerMode); - /** + int setScanTrigger(ScanTriggerMode scanTriggerMode); + /** * Set the interval for scanning capacitive channels when the * device's scanning is triggered by the timer. * @@ -367,16 +386,16 @@ class Trill : public I2c * \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); - /** + 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); - /** + int setAutoScanInterval(uint16_t interval); + /** * Set how the EVT pin behaves. * * @param mode an #EventMode denoting the required behaviour. @@ -384,8 +403,8 @@ class Trill : public I2c * \copydoc TAGS_canonical_return * \copydoc TAGS_firmware_3_error */ - int setEventMode(EventMode mode); - /** + int setEventMode(EventMode mode); + /** * Set a channel mask identifying which scanning channels are * enabled. * @@ -396,8 +415,8 @@ class Trill : public I2c * \copydoc TAGS_canonical_return * \copydoc TAGS_firmware_3_error */ - int setChannelMask(uint32_t mask); - /** + int setChannelMask(uint32_t mask); + /** * Set the format used for transmission of non-centroid data * from the device to the host. * @@ -409,13 +428,13 @@ class Trill : public I2c * \copydoc TAGS_canonical_return * \copydoc TAGS_firmware_3_error */ - int setTransmissionFormat(uint8_t width, uint8_t shift); - /** @} */ // end of Scan Configuration Settings - /** + 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). @@ -428,8 +447,8 @@ class Trill : public I2c * * \copydoc TAGS_firmware_3_undef */ - int readStatusByte(); - /** + int readStatusByte(); + /** * Whether the device has reset since a identify command was * last written to it. * @@ -437,23 +456,23 @@ class Trill : public I2c * * \copydoc TAGS_firmware_3_error */ - bool hasReset(); - /** + 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(); - /** + bool hasActivity(); + /** * Get the frameId. * This relies on a current status byte. * * \copydoc TAGS_firmware_3_undef */ - uint8_t getFrameId(); - /** + 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. @@ -464,12 +483,12 @@ class Trill : public I2c * @return the counter * \copydoc TAGS_firmware_3_undef */ - uint32_t getFrameIdUnwrapped(); - /** + uint32_t getFrameIdUnwrapped(); + /** * @} */ - /** + /** * @name Centroid Mode * @{ * @@ -499,38 +518,41 @@ class Trill : public I2c * @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(); - /** + 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(); - /** + bool is2D(); + /** * Return the number of bytes to read when reading data. */ - unsigned int getBytesToRead(bool includesStatusByte); - /** + unsigned int getBytesToRead(bool includesStatusByte); + /** * Return the number of "button" channels on the device. */ - unsigned int getNumButtons() { return 2 * (getMode() == CENTROID && RING == deviceType());}; - /** + 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(); - /** + unsigned int getNumTouches(); + /** * Get the location of a touch on the vertical axis of the * device. * @@ -541,8 +563,8 @@ class Trill : public I2c * @return the position of the touch relative to the axis, or * -1 if no such touch exists. */ - float touchLocation(uint8_t touch_num); - /** + float touchLocation(uint8_t touch_num); + /** * Get the size of a touch. * * \copydoc TAGS_1d @@ -550,15 +572,15 @@ class Trill : public I2c * @return the size of the touch, if the touch exists, or 0 * otherwise. */ - float touchSize(uint8_t touch_num); - /** + 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(); - /** + unsigned int getNumHorizontalTouches(); + /** * Get the location of a touch on the horizontal axis of the * device. * @@ -569,8 +591,8 @@ class Trill : public I2c * @return the position of the touch relative to the axis, or * -1 if no such touch exists. * */ - float touchHorizontalLocation(uint8_t touch_num); - /** + float touchHorizontalLocation(uint8_t touch_num); + /** * Get the size of a touch. * * \copydoc TAGS_2d @@ -578,29 +600,29 @@ class Trill : public I2c * @return the size of the touch, if the touch exists, or 0 * otherwise. */ - float touchHorizontalSize(uint8_t touch_num); - /** + float touchHorizontalSize(uint8_t touch_num); + /** * Get the vertical location of the compound touch on the * device. * * \copydoc TAGS_1d * */ - float compoundTouchLocation(); - /** + float compoundTouchLocation(); + /** * Get the horizontal location of the compound touch on the * device. * * \copydoc TAGS_1d */ - float compoundTouchHorizontalLocation(); - /** + float compoundTouchHorizontalLocation(); + /** * Get the size of the compound touch on the * device. * * \copydoc TAGS_1d */ - float compoundTouchSize(); - /** + float compoundTouchSize(); + /** * Get the value of the capacitive "button" channels on the * device * @@ -609,7 +631,7 @@ class Trill : public I2c * @return The differential reading on the button, normalised * between 0 and 1. */ - float getButtonValue(uint8_t button_num); + float getButtonValue(uint8_t button_num); - /** @}*/ // end of centroid mode + /** @}*/ // end of centroid mode }; diff --git a/src/dev/trill/calculateCentroids.h b/src/dev/trill/calculateCentroids.h index 54e919308..6e8d7bc50 100644 --- a/src/dev/trill/calculateCentroids.h +++ b/src/dev/trill/calculateCentroids.h @@ -1,125 +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 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; + WORD lastSensorVal, currentSensorVal, currentWeightedSum, + currentUnweightedSum; + BYTE currentStart, currentLength; - for(sensorIndex = 0; sensorIndex < maxNumCentroids; sensorIndex++) { - centroidBuffer[sensorIndex] = 0xFFFF; - sizeBuffer[sensorIndex] = 0; - } + for(sensorIndex = 0; sensorIndex < maxNumCentroids; sensorIndex++) + { + centroidBuffer[sensorIndex] = 0xFFFF; + sizeBuffer[sensorIndex] = 0; + } - currentSensorVal = 0; + currentSensorVal = 0; - for(sensorIndex = 0, actualHardwareIndex = minSensor; sensorIndex < numSensors; sensorIndex++) - { - lastSensorVal = currentSensorVal; + 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; - } + 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(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++; - } + 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; - } + 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(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 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; - } - } + 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; - } - } + 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++; - } + // 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; + return (lastActiveSensor << 8) | currentStart; }