From e5dd4bc16f0b95e7b645be7130f4713d59f5ace6 Mon Sep 17 00:00:00 2001 From: Matt Grau Date: Mon, 1 Jul 2019 21:28:14 +0200 Subject: [PATCH 1/3] implement reading and writing mode add functions getMode() and setMeasurementMode() also add private function unsetConfigMode() to revert back from configuration mode. --- src/TFMini.cpp | 59 +++++++++++++++++++++++++++++++++++++++++++++++++- src/TFMini.h | 11 ++++++++++ 2 files changed, 69 insertions(+), 1 deletion(-) diff --git a/src/TFMini.cpp b/src/TFMini.cpp index b772460..d14d813 100644 --- a/src/TFMini.cpp +++ b/src/TFMini.cpp @@ -33,6 +33,7 @@ boolean TFMini::begin(Stream* _streamPtr) { // Clear state distance = -1; strength = -1; + mode = -1; state = READY; // Set standard output mode @@ -73,6 +74,11 @@ uint16_t TFMini::getRecentSignalStrength() { return strength; } +// Public: Return the measurement mode +uint8_t TFMini::getMode() { + return mode; +} + // Private: Set the TF Mini into the correct measurement mode void TFMini::setStandardOutputMode() { @@ -97,7 +103,21 @@ void TFMini::setConfigMode() { streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x01); + streamPtr->write((uint8_t)0x02); + delay(100); +} + +// Exit configuration mode +void TFMini::unsetConfigMode() { + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x02); + delay(100); } // Set single scan mode (external trigger) @@ -112,6 +132,39 @@ void TFMini::setSingleScanMode() { streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x40); + delay(100); + unsetConfigMode(); +} + +// Set measurement mode +void TFMini::setMeasurementMode(MODE _mode) { + setConfigMode(); + + // fix detection pattern + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x01); + streamPtr->write((uint8_t)0x14); + + delay(100); + + // setting measurement mode + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)_mode); + streamPtr->write((uint8_t)0x11); + + delay(100); + + unsetConfigMode(); } // Send external trigger @@ -126,6 +179,8 @@ void TFMini::externalTrigger() { streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x41); + delay(100); + unsetConfigMode(); } // Private: Handles the low-level bits of communicating with the TFMini, and detecting some communication errors. @@ -155,7 +210,8 @@ int TFMini::takeMeasurement() { if (numCharsRead > TFMINI_MAXBYTESBEFOREHEADER) { state = ERROR_SERIAL_NOHEADER; distance = -1; - strength = -1; + strength = -1; + mode = -1; if (TFMINI_DEBUGMODE == 1) Serial.println("ERROR: no header"); return -1; } @@ -201,6 +257,7 @@ int TFMini::takeMeasurement() { // Step 4: Store values distance = dist; strength = st; + mode = reserved; state = MEASUREMENT_OK; // Return success diff --git a/src/TFMini.h b/src/TFMini.h index 4c6e2cc..82e250a 100644 --- a/src/TFMini.h +++ b/src/TFMini.h @@ -43,6 +43,13 @@ derived from this software without specific prior written permission. #define ERROR_SERIAL_TOOMANYTRIES 3 #define MEASUREMENT_OK 10 +enum MODE { + ZERO=0x00, // for 0-2m + SHORT=0x02, // for 0-5m + MIDDLE=0x03, // for 0.5-5m + LONG=0x07 // for 1-12m +}; + // // Driver Class Definition @@ -54,10 +61,12 @@ class TFMini { // Configuration boolean begin(Stream* _streamPtr); void setSingleScanMode(); + void setMeasurementMode(MODE _mode); // Data collection uint16_t getDistance(); uint16_t getRecentSignalStrength(); + uint8_t getMode(); void externalTrigger(); private: @@ -65,10 +74,12 @@ class TFMini { int state; uint16_t distance; uint16_t strength; + uint8_t mode; // Low-level communication void setStandardOutputMode(); void setConfigMode(); + void unsetConfigMode(); int takeMeasurement(); }; From 0acfbdecd13b85a51f21f0a0c70c4f370a3b9cad Mon Sep 17 00:00:00 2001 From: Matt Grau Date: Mon, 1 Jul 2019 21:32:41 +0200 Subject: [PATCH 2/3] format using clang-format --- src/TFMini.cpp | 150 ++++++++++++++++++++++++++++--------------------- src/TFMini.h | 90 +++++++++++++++-------------- 2 files changed, 131 insertions(+), 109 deletions(-) diff --git a/src/TFMini.cpp b/src/TFMini.cpp index d14d813..22d4fb3 100644 --- a/src/TFMini.cpp +++ b/src/TFMini.cpp @@ -21,12 +21,13 @@ derived from this software without specific prior written permission. #include "TFMini.h" // Constructor -TFMini::TFMini() { +TFMini::TFMini() +{ // Empty constructor } - -boolean TFMini::begin(Stream* _streamPtr) { +boolean TFMini::begin(Stream *_streamPtr) +{ // Store reference to stream/serial object streamPtr = _streamPtr; @@ -35,53 +36,63 @@ boolean TFMini::begin(Stream* _streamPtr) { strength = -1; mode = -1; state = READY; - + // Set standard output mode setStandardOutputMode(); - + return true; } - -// Public: The main function to measure distance. -uint16_t TFMini::getDistance() { +// Public: The main function to measure distance. +uint16_t TFMini::getDistance() +{ int numMeasurementAttempts = 0; - while (takeMeasurement() != 0) { + while (takeMeasurement() != 0) + { numMeasurementAttempts += 1; - if (numMeasurementAttempts > TFMINI_MAX_MEASUREMENT_ATTEMPTS) { - Serial.println ("TF Mini error: too many measurement attempts"); - Serial.println ("Last error:"); - if (state == ERROR_SERIAL_NOHEADER) Serial.println("ERROR_SERIAL_NOHEADER"); - if (state == ERROR_SERIAL_BADCHECKSUM) Serial.println("ERROR_SERIAL_BADCHECKSUM"); - if (state == ERROR_SERIAL_TOOMANYTRIES) Serial.println("ERROR_SERIAL_TOOMANYTRIES"); - + if (numMeasurementAttempts > TFMINI_MAX_MEASUREMENT_ATTEMPTS) + { + Serial.println("TF Mini error: too many measurement attempts"); + Serial.println("Last error:"); + if (state == ERROR_SERIAL_NOHEADER) + Serial.println("ERROR_SERIAL_NOHEADER"); + if (state == ERROR_SERIAL_BADCHECKSUM) + Serial.println("ERROR_SERIAL_BADCHECKSUM"); + if (state == ERROR_SERIAL_TOOMANYTRIES) + Serial.println("ERROR_SERIAL_TOOMANYTRIES"); + state = ERROR_SERIAL_TOOMANYTRIES; distance = -1; - strength = -1; - return -1; + strength = -1; + return -1; } } - if (state == MEASUREMENT_OK) { + if (state == MEASUREMENT_OK) + { return distance; - } else { + } + else + { return -1; } } // Public: Return the most recent signal strength measuremenet from the TF Mini -uint16_t TFMini::getRecentSignalStrength() { +uint16_t TFMini::getRecentSignalStrength() +{ return strength; } // Public: Return the measurement mode -uint8_t TFMini::getMode() { +uint8_t TFMini::getMode() +{ return mode; } - // Private: Set the TF Mini into the correct measurement mode -void TFMini::setStandardOutputMode() { +void TFMini::setStandardOutputMode() +{ // Set to "standard" output mode (this is found in the debug documents) streamPtr->write((uint8_t)0x42); streamPtr->write((uint8_t)0x57); @@ -94,7 +105,8 @@ void TFMini::setStandardOutputMode() { } // Set configuration mode -void TFMini::setConfigMode() { +void TFMini::setConfigMode() +{ // advanced parameter configuration mode streamPtr->write((uint8_t)0x42); streamPtr->write((uint8_t)0x57); @@ -108,7 +120,8 @@ void TFMini::setConfigMode() { } // Exit configuration mode -void TFMini::unsetConfigMode() { +void TFMini::unsetConfigMode() +{ streamPtr->write((uint8_t)0x42); streamPtr->write((uint8_t)0x57); streamPtr->write((uint8_t)0x02); @@ -116,12 +129,13 @@ void TFMini::unsetConfigMode() { streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); - streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x02); delay(100); } // Set single scan mode (external trigger) -void TFMini::setSingleScanMode() { +void TFMini::setSingleScanMode() +{ setConfigMode(); // setting trigger source to external streamPtr->write((uint8_t)0x42); @@ -137,7 +151,8 @@ void TFMini::setSingleScanMode() { } // Set measurement mode -void TFMini::setMeasurementMode(MODE _mode) { +void TFMini::setMeasurementMode(MODE _mode) +{ setConfigMode(); // fix detection pattern @@ -168,8 +183,9 @@ void TFMini::setMeasurementMode(MODE _mode) { } // Send external trigger -void TFMini::externalTrigger() { - setConfigMode(); +void TFMini::externalTrigger() +{ + setConfigMode(); // send trigger streamPtr->write((uint8_t)0x42); streamPtr->write((uint8_t)0x57); @@ -184,76 +200,85 @@ void TFMini::externalTrigger() { } // Private: Handles the low-level bits of communicating with the TFMini, and detecting some communication errors. -int TFMini::takeMeasurement() { +int TFMini::takeMeasurement() +{ int numCharsRead = 0; - uint8_t lastChar = 0x00; - + uint8_t lastChar = 0x00; + // Step 1: Read the serial stream until we see the beginning of the TF Mini header, or we timeout reading too many characters. - while (1) { + while (1) + { - if (streamPtr->available()) { + if (streamPtr->available()) + { uint8_t curChar = streamPtr->read(); - if ((lastChar == 0x59) && (curChar == 0x59)) { + if ((lastChar == 0x59) && (curChar == 0x59)) + { // Break to begin frame break; - - } else { - // We have not seen two 0x59's in a row -- store the current character and continue reading. + } + else + { + // We have not seen two 0x59's in a row -- store the current character and continue reading. lastChar = curChar; - numCharsRead += 1; - } + numCharsRead += 1; + } } - // Error detection: If we read more than X characters without finding a frame header, then it's likely there is an issue with - // the Serial connection, and we should timeout and throw an error. - if (numCharsRead > TFMINI_MAXBYTESBEFOREHEADER) { + // Error detection: If we read more than X characters without finding a frame header, then it's likely there is an issue with + // the Serial connection, and we should timeout and throw an error. + if (numCharsRead > TFMINI_MAXBYTESBEFOREHEADER) + { state = ERROR_SERIAL_NOHEADER; distance = -1; - strength = -1; - mode = -1; - if (TFMINI_DEBUGMODE == 1) Serial.println("ERROR: no header"); - return -1; + strength = -1; + mode = -1; + if (TFMINI_DEBUGMODE == 1) + Serial.println("ERROR: no header"); + return -1; } - } - + // Step 2: Read one frame from the TFMini uint8_t frame[TFMINI_FRAME_SIZE]; uint8_t checksum = 0x59 + 0x59; - for (int i=0; iavailable()) { + while (!streamPtr->available()) + { // wait for a character to become available - } + } frame[i] = streamPtr->read(); // Store running checksum - if (i < TFMINI_FRAME_SIZE-2) { + if (i < TFMINI_FRAME_SIZE - 2) + { checksum += frame[i]; } } // Step 2A: Compare checksum - // Last byte in the frame is an 8-bit checksum - uint8_t checksumByte = frame[TFMINI_FRAME_SIZE-1]; - if (checksum != checksumByte) { + // Last byte in the frame is an 8-bit checksum + uint8_t checksumByte = frame[TFMINI_FRAME_SIZE - 1]; + if (checksum != checksumByte) + { state = ERROR_SERIAL_BADCHECKSUM; distance = -1; strength = -1; - if (TFMINI_DEBUGMODE == 1) Serial.println("ERROR: bad checksum"); + if (TFMINI_DEBUGMODE == 1) + Serial.println("ERROR: bad checksum"); return -1; } - // Step 3: Interpret frame uint16_t dist = (frame[1] << 8) + frame[0]; uint16_t st = (frame[3] << 8) + frame[2]; uint8_t reserved = frame[4]; uint8_t originalSignalQuality = frame[5]; - // Step 4: Store values distance = dist; strength = st; @@ -261,6 +286,5 @@ int TFMini::takeMeasurement() { state = MEASUREMENT_OK; // Return success - return 0; + return 0; } - diff --git a/src/TFMini.h b/src/TFMini.h index 82e250a..b52c0fb 100644 --- a/src/TFMini.h +++ b/src/TFMini.h @@ -19,68 +19,66 @@ derived from this software without specific prior written permission. */ #if (ARDUINO >= 100) - #include "Arduino.h" +#include "Arduino.h" #else - #include "WProgram.h" +#include "WProgram.h" #endif - // Defines -#define TFMINI_BAUDRATE 115200 -#define TFMINI_DEBUGMODE 0 +#define TFMINI_BAUDRATE 115200 +#define TFMINI_DEBUGMODE 0 // The frame size is nominally 9 characters, but we don't include the first two 0x59's marking the start of the frame -#define TFMINI_FRAME_SIZE 7 +#define TFMINI_FRAME_SIZE 7 // Timeouts -#define TFMINI_MAXBYTESBEFOREHEADER 30 -#define TFMINI_MAX_MEASUREMENT_ATTEMPTS 10 +#define TFMINI_MAXBYTESBEFOREHEADER 30 +#define TFMINI_MAX_MEASUREMENT_ATTEMPTS 10 // States -#define READY 0 -#define ERROR_SERIAL_NOHEADER 1 -#define ERROR_SERIAL_BADCHECKSUM 2 -#define ERROR_SERIAL_TOOMANYTRIES 3 -#define MEASUREMENT_OK 10 +#define READY 0 +#define ERROR_SERIAL_NOHEADER 1 +#define ERROR_SERIAL_BADCHECKSUM 2 +#define ERROR_SERIAL_TOOMANYTRIES 3 +#define MEASUREMENT_OK 10 -enum MODE { - ZERO=0x00, // for 0-2m - SHORT=0x02, // for 0-5m - MIDDLE=0x03, // for 0.5-5m - LONG=0x07 // for 1-12m +enum MODE +{ + ZERO = 0x00, // for 0-2m + SHORT = 0x02, // for 0-5m + MIDDLE = 0x03, // for 0.5-5m + LONG = 0x07 // for 1-12m }; - // // Driver Class Definition // -class TFMini { - public: - TFMini(void); +class TFMini +{ +public: + TFMini(void); - // Configuration - boolean begin(Stream* _streamPtr); - void setSingleScanMode(); - void setMeasurementMode(MODE _mode); - - // Data collection - uint16_t getDistance(); - uint16_t getRecentSignalStrength(); - uint8_t getMode(); - void externalTrigger(); + // Configuration + boolean begin(Stream *_streamPtr); + void setSingleScanMode(); + void setMeasurementMode(MODE _mode); - private: - Stream* streamPtr; - int state; - uint16_t distance; - uint16_t strength; - uint8_t mode; - - // Low-level communication - void setStandardOutputMode(); - void setConfigMode(); - void unsetConfigMode(); - int takeMeasurement(); - -}; + // Data collection + uint16_t getDistance(); + uint16_t getRecentSignalStrength(); + uint8_t getMode(); + void externalTrigger(); +private: + Stream *streamPtr; + int state; + uint16_t distance; + uint16_t strength; + uint8_t mode; + + // Low-level communication + void setStandardOutputMode(); + void setConfigMode(); + void unsetConfigMode(); + int takeMeasurement(); +}; From d787b927a8d85f98db178b9a3ee6205b812f1601 Mon Sep 17 00:00:00 2001 From: Matt Grau Date: Sun, 22 Sep 2019 18:44:20 +0200 Subject: [PATCH 3/3] add functions to set RangeLimit and SignalThreshold --- src/TFMini.cpp | 52 +++++++++++++++++++++++++++++++++++++++++++++++++- src/TFMini.h | 2 ++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/TFMini.cpp b/src/TFMini.cpp index 22d4fb3..dfc35f9 100644 --- a/src/TFMini.cpp +++ b/src/TFMini.cpp @@ -1,6 +1,7 @@ /* Arduino driver for Benewake TFMini time-of-flight distance sensor. by Peter Jansen (December 11/2017) +modified for EAWAG volaser (2019) This code is open source software in the public domain. THIS SOFTWARE IS PROVIDED ''AS IS'' AND ANY @@ -162,7 +163,7 @@ void TFMini::setMeasurementMode(MODE _mode) streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); streamPtr->write((uint8_t)0x00); - streamPtr->write((uint8_t)0x01); + streamPtr->write((uint8_t)0x01); //0x00=unlocked, 0x01=locked streamPtr->write((uint8_t)0x14); delay(100); @@ -182,6 +183,55 @@ void TFMini::setMeasurementMode(MODE _mode) unsetConfigMode(); } +// Set maximum distance +void TFMini::setRangeLimit(uint16_t range) { + setConfigMode(); + + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)(range & 0xff)); + streamPtr->write((uint8_t)((range >> 8) & 0xff)); + streamPtr->write((uint8_t)(range > 0 ? 0x01 : 0x00)); // 0x00=range limit disabled, 0x01=enabled + streamPtr->write((uint8_t)0x19); + + delay(100); + + unsetConfigMode(); +} + +// Set maximum distance +void TFMini::setSignalThreshold(uint8_t min, uint16_t max) { + setConfigMode(); + + // Set minimum signal strength + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)min); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)0x20); + + delay(100); + + // Set maximum signal strength + streamPtr->write((uint8_t)0x42); + streamPtr->write((uint8_t)0x57); + streamPtr->write((uint8_t)0x02); + streamPtr->write((uint8_t)0x00); + streamPtr->write((uint8_t)(max & 0xff)); + streamPtr->write((uint8_t)((max >> 8) & 0xff)); + streamPtr->write((uint8_t)0x1d); // distance to be returned if max signal is exceeded + streamPtr->write((uint8_t)0x21); + + delay(100); + + unsetConfigMode(); +} + // Send external trigger void TFMini::externalTrigger() { diff --git a/src/TFMini.h b/src/TFMini.h index b52c0fb..cbe9ef4 100644 --- a/src/TFMini.h +++ b/src/TFMini.h @@ -62,6 +62,8 @@ class TFMini boolean begin(Stream *_streamPtr); void setSingleScanMode(); void setMeasurementMode(MODE _mode); + void setRangeLimit(uint16_t range); + void setSignalThreshold(uint8_t min, uint16_t max); // Data collection uint16_t getDistance();