Trill: read from the device waiting for an ack instead of blindly sle…

…eping (if fw supports)
BelaPlatform · Dec 8, 2023 · fc131b7 · fc131b7
1 parent db1a980
commit fc131b7
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 14 deletions.
diff --git a/libraries/Trill/Trill.cpp b/libraries/Trill/Trill.cpp
@@ -28,6 +28,7 @@ enum {
 	kCommandReset = 12,
 	kCommandFormat = 13,
 	kCommandScanTrigger = 16,
+	kCommandAck = 254,
 	kCommandIdentify = 255
 };
 
@@ -141,9 +142,10 @@ int Trill::setup(unsigned int i2c_bus, Device device, uint8_t i2c_address)
 		return 1;
 	}
 
-	// disable scanning
-	if(setScanTrigger(kScanTriggerDisabled))
-		return 1;
+	// 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;
@@ -314,8 +316,7 @@ int Trill::writeCommandAndHandle(const i2c_char_t* data, size_t size, const char
 		return 1;
 	}
 	currentReadOffset = buf[0];
-	usleep(commandSleepTime); // need to give enough time to process command
-	return 0;
+	return waitForAck(buf[1], name);
 }
 
 int Trill::readBytesFrom(const uint8_t offset, i2c_char_t& byte, const char* name)
@@ -345,7 +346,42 @@ int Trill::readBytesFrom(const uint8_t offset, i2c_char_t* data, size_t size, co
 		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;
+	}
+	i2c_char_t buf[2]; // TODO: make it of size 1
+	unsigned int sleep = commandSleepTime;
+	unsigned int totalSleep = 0;
+	int verbose = 0;
+	while(totalSleep < 100000)
+	{
+		usleep(sleep);
+		if(readBytesFrom(kOffsetCommand, buf, sizeof(buf), name))
+			return 1;
+		if(kCommandAck == buf[0])
+		{
+			// check second byte if not identify
+			if(kCommandIdentify == command || buf[1] == command) // debug only. TODOL remove
+			{
 
+				verbose && printf("Ack'ed %d with %d %d\n", command, buf[0], buf[1]);
+				return 0;
+			}
+		}
+		verbose && printf("sleep %d: %d %d\n", sleep, buf[0], buf[1]);
+		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) \
@@ -356,10 +392,10 @@ int Trill::readBytesFrom(const uint8_t offset, i2c_char_t* data, size_t size, co
 	}
 
 int Trill::identify() {
-	if(WRITE_COMMAND(kCommandIdentify))
-		return 1;
-
-	uint8_t rbuf[3];
+	// 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;
@@ -379,7 +415,6 @@ int Trill::identify() {
 	}
 	device_type_ = readDeviceType;
 	firmware_version_ = rbuf[2];
-	// rbuf[3] is currently unused
 
 	return 0;
 }

diff --git a/libraries/Trill/Trill.h b/libraries/Trill/Trill.h
@@ -53,11 +53,11 @@ class Trill : public I2c
 		uint32_t frameId;
 		uint8_t statusByte;
 		uint8_t address;
-		uint8_t firmware_version_; // Firmware version running on the device
+		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<uint8_t> dataBuffer;
-		uint16_t commandSleepTime = 10000;
+		uint16_t commandSleepTime = 1000;
 		size_t currentReadOffset = -1;
 		bool shouldReadFrameId = false;
 		unsigned int numBits;
@@ -77,6 +77,7 @@ class Trill : public I2c
 		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);
 		bool readErrorOccurred;
 	public:
@@ -333,8 +334,9 @@ class Trill : public I2c
 		 */
 		int readStatusByte();
 		/**
-		 * Whether the device has reset since a command was last
-		 * written to it.
+		 * Whether the device has reset since a identify command was
+		 * last written to it.
+		 *
 		 * This relies on a current status byte.
 		 */
 		bool hasReset();