Fix race conditions, resource leaks, and performance issues

main/src/Screens/DriveScreen/DriveScreen.cpp

@@ -171,34 +171,40 @@ void DriveScreen::preDraw(){
 	leftMotorSpeedLabel.setText(std::to_string(cachedMotorSpeeds.x));
 	rightMotorSpeedLabel.setText(std::to_string(cachedMotorSpeeds.y));
 
-	if(cachedMotorSpeeds.x == 0 && cachedMotorSpeeds.y == 0){
-		arrowUp.setPath("/spiffs/drive/arrow-up.raw");
-		arrowRight.setPath("/spiffs/drive/arrow-right.raw");
-		arrowDown.setPath("/spiffs/drive/arrow-down.raw");
-		arrowLeft.setPath("/spiffs/drive/arrow-left.raw");
-	}else{
-		if(cachedDriveDir == 7 || cachedDriveDir == 0 || cachedDriveDir == 1){
-			arrowUp.setPath("/spiffs/drive/arrow-up-active.raw");
-		}else{
-			arrowUp.setPath("/spiffs/drive/arrow-up.raw");
-		}
+	bool motorZero = (cachedMotorSpeeds.x == 0 && cachedMotorSpeeds.y == 0);
+	if(motorZero != lastRenderedMotorZero || (!motorZero && cachedDriveDir != lastRenderedDriveDir)){
+		lastRenderedMotorZero = motorZero;
+		lastRenderedDriveDir = cachedDriveDir;
 
-		if(cachedDriveDir == 1 || cachedDriveDir == 2 || cachedDriveDir == 3){
-			arrowRight.setPath("/spiffs/drive/arrow-right-active.raw");
-		}else{
+		if(motorZero){
+			arrowUp.setPath("/spiffs/drive/arrow-up.raw");
 			arrowRight.setPath("/spiffs/drive/arrow-right.raw");
-		}
-
-		if(cachedDriveDir == 3 || cachedDriveDir == 4 || cachedDriveDir == 5){
-			arrowDown.setPath("/spiffs/drive/arrow-down-active.raw");
-		}else{
 			arrowDown.setPath("/spiffs/drive/arrow-down.raw");
-		}
-
-		if(cachedDriveDir == 5 || cachedDriveDir == 6 || cachedDriveDir == 7){
-			arrowLeft.setPath("/spiffs/drive/arrow-left-active.raw");
-		}else{
 			arrowLeft.setPath("/spiffs/drive/arrow-left.raw");
+		}else{
+			if(cachedDriveDir == 7 || cachedDriveDir == 0 || cachedDriveDir == 1){
+				arrowUp.setPath("/spiffs/drive/arrow-up-active.raw");
+			}else{
+				arrowUp.setPath("/spiffs/drive/arrow-up.raw");
+			}
+
+			if(cachedDriveDir == 1 || cachedDriveDir == 2 || cachedDriveDir == 3){
+				arrowRight.setPath("/spiffs/drive/arrow-right-active.raw");
+			}else{
+				arrowRight.setPath("/spiffs/drive/arrow-right.raw");
+			}
+
+			if(cachedDriveDir == 3 || cachedDriveDir == 4 || cachedDriveDir == 5){
+				arrowDown.setPath("/spiffs/drive/arrow-down-active.raw");
+			}else{
+				arrowDown.setPath("/spiffs/drive/arrow-down.raw");
+			}
+
+			if(cachedDriveDir == 5 || cachedDriveDir == 6 || cachedDriveDir == 7){
+				arrowLeft.setPath("/spiffs/drive/arrow-left-active.raw");
+			}else{
+				arrowLeft.setPath("/spiffs/drive/arrow-left.raw");
+			}
 		}
 	}
 
@@ -260,7 +266,7 @@ void DriveScreen::onLoop(){
 			if(evt.facility == Facility::TCP){
 				if(const auto data = (TCPClient::Event*) evt.data){
 					if(data->status == TCPClient::Event::Status::Disconnected){
-						free(evt.data);
+						if(!evt.isInline) free(evt.data);
 						transition([](Sprite& canvas){ return std::make_unique<PairScreen>(canvas, true); });
 						return;
 					}
@@ -270,12 +276,12 @@ void DriveScreen::onLoop(){
 				auto data = (Battery::Event*) evt.data;
 				if(data->level == Battery::Critical){
 					shutdown();
-					free(evt.data);
+					if(!evt.isInline) free(evt.data);
 					return;
 				}
 			}
 
-			free(evt.data);
+			if(!evt.isInline) free(evt.data);
 		}
 	}
 
@@ -529,7 +535,6 @@ void DriveScreen::setupControl(){
 	Events::listen(Facility::Encoders, &evts);
 	Events::listen(Facility::RoverState, &evts);
 	Events::listen(Facility::Potentiometers, &evts);
-	Events::listen(Facility::RoverState, &evts);
 
 	comm.sendModulesEnable(true);
 
@@ -571,7 +576,7 @@ void DriveScreen::checkEvents(){
 			processPotentiometers(*data);
 		}
 
-		free(evt.data);
+		if(!evt.isInline) free(evt.data);
 	}
 }
 

main/src/Screens/DriveScreen/DriveScreen.h

@@ -66,6 +66,8 @@ class DriveScreen : public Screen {
 
 	glm::vec<2, int8_t> cachedMotorSpeeds = { 0, 0 };
 	uint8_t cachedDriveDir = 0;
+	uint8_t lastRenderedDriveDir = 0xFF;
+	bool lastRenderedMotorZero = true;
 
 	bool armEnabled;
 	uint8_t pinchPos = 50;

main/src/Services/Feed.cpp

@@ -2,6 +2,7 @@
 #include "Util/stdafx.h"
 #include <esp_log.h>
 #include <mutex>
+#include <sys/select.h>
 
 static const char* tag = "Feed";
 
@@ -34,29 +35,43 @@ bool Feed::nextFrame(std::function<void(const DriveInfo& info, const Color* img)
 	std::unique_lock lock(readyFrameMut);
 	if(readyFrame.imgIndex == -1) return false;
 
-	volatile int frameIndex = readyFrame.imgIndex;
+	int frameIndex = readyFrame.imgIndex;
 	if(frameIndex == -1) return false;
 
-	volatile const DriveInfo info = readyFrame.info;
-	volatile const Color* img = frameImgs[frameIndex].data();
+	const DriveInfo info = readyFrame.info;
+	const Color* img = frameImgs[frameIndex].data();
 
 	readyFrame.imgIndex = -1;
 	lock.unlock();
 
-	cb((const DriveInfo&) info, (const Color*) img);
+	cb(info, img);
 
 	freeImgs[frameIndex] = true;
 
 	return true;
 }
 
 void Feed::readLoop(){
-	int bytes = udp.read(readBuf.data(), readBuf.size());
-	if(bytes <= 0){
-		delayMillis(10);
+	int udpSock = udp.getSock();
+	if(udpSock == -1){
+		delayMillis(100);
 		return;
 	}
 
+	fd_set readfds;
+	FD_ZERO(&readfds);
+	FD_SET(udpSock, &readfds);
+
+	timeval tv;
+	tv.tv_sec = 0;
+	tv.tv_usec = 100000; // 100 ms
+
+	int sel = select(udpSock + 1, &readfds, nullptr, nullptr, &tv);
+	if(sel <= 0) return;
+
+	int bytes = udp.read(readBuf.data(), readBuf.size());
+	if(bytes <= 0) return;
+
 	std::lock_guard lock(rxMut);
 	rxBuf.write(readBuf.data(), bytes);
 	dataAvailable.release();

main/src/Services/Feed.h

@@ -37,7 +37,7 @@ class Feed {
 	void decodeLoop();
 	bool findFrame();
 
-	bool freeImgs[3] = { true, true, true };
+	std::atomic<bool> freeImgs[3] = { true, true, true };
 	std::vector<Color> frameImgs[3];
 
 	std::mutex readyFrameMut;

main/src/Services/TCPClient.cpp

@@ -2,6 +2,7 @@
 #include "Util/Events.h"
 #include <lwip/sockets.h>
 #include <esp_log.h>
+#include <sys/select.h>
 
 static const char* TAG = "TCPClient";
 
@@ -77,7 +78,15 @@ bool TCPClient::read(uint8_t* buf, size_t count){
 			return false;
 		}else if(now < 0){
 			if(errno == EAGAIN || errno == EWOULDBLOCK){
-				vTaskDelay(1);
+				fd_set fds;
+				FD_ZERO(&fds);
+				FD_SET(sock, &fds);
+				timeval tv = { .tv_sec = 1, .tv_usec = 0 };
+				int sel = select(sock + 1, &fds, nullptr, nullptr, &tv);
+				if(sel <= 0){
+					disconnect();
+					return false;
+				}
 				continue;
 			}else{
 				disconnect();
@@ -108,7 +117,15 @@ bool TCPClient::write(uint8_t* data, size_t count){
 			return false;
 		}else if(now < 0){
 			if(errno == EAGAIN || errno == EWOULDBLOCK){
-				vTaskDelay(1);
+				fd_set fds;
+				FD_ZERO(&fds);
+				FD_SET(sock, &fds);
+				timeval tv = { .tv_sec = 1, .tv_usec = 0 };
+				int sel = select(sock + 1, nullptr, &fds, nullptr, &tv);
+				if(sel <= 0){
+					disconnect();
+					return false;
+				}
 				continue;
 			}else{
 				disconnect();

main/src/Services/UDPListener.cpp

@@ -17,6 +17,7 @@ UDPListener::UDPListener(){
 	inet_pton(AF_INET, "11.0.0.2", &addr.sin_addr);
 	if(bind(sock, (sockaddr*) &addr, sizeof(addr)) != 0){
 		ESP_LOGE(TAG, "Can't bind address to socket, errno=%d: %s", errno, strerror(errno));
+		close(sock);
 		sock = -1;
 		return;
 	}

main/src/Services/UDPListener.h

@@ -10,6 +10,7 @@ class UDPListener {
 	virtual ~UDPListener();
 
 	int read(uint8_t* buf, size_t count);
+	int getSock() const { return sock; }
 
 private:
 	int sock = -1;

main/src/Util/Events.cpp

@@ -19,21 +19,13 @@ void Events::unlisten(EventQueue* queue){
 }
 
 void Events::post(Facility facility, const void* data, size_t size){
-	std::unique_lock lock(mut);
+	std::lock_guard lock(mut);
 
 	auto pair = queues.find(facility);
 	if(pair == queues.end()) return;
 
-	const std::unordered_set<EventQueue*> subs = pair->second;
-	lock.unlock();
-
-	for(auto queue : subs){
-		void* qData = nullptr;
-		if(size != 0){
-			qData = malloc(size);
-			memcpy(qData, data, size);
-		}
-		queue->post(facility, qData);
+	for(auto queue : pair->second){
+		queue->post(facility, data, size);
 	}
 }
 
@@ -55,27 +47,44 @@ bool EventQueue::get(Event& event, TickType_t timeout){
 	if(internal.killPill) return false;
 
 	event = internal.evt;
+	if(event.isInline){
+		event.data = event.inlineData;
+	}
 
 	return true;
 }
 
-bool EventQueue::post(Facility facility, void* data){
+bool EventQueue::post(Facility facility, const void* data, size_t size){
 	InternalEvent event = {
 		.evt = {
 				.facility = facility,
-				.data = data
+				.data = nullptr,
+				.inlineData = {},
+				.isInline = (size <= sizeof(Event::inlineData))
 		},
 		.killPill = false
 	};
 
+	if(size > 0){
+		if(event.evt.isInline){
+			memcpy(event.evt.inlineData, data, size);
+			event.evt.data = event.evt.inlineData;
+		}else{
+			event.evt.data = malloc(size);
+			memcpy(event.evt.data, data, size);
+		}
+	}
+
 	return xQueueSend(queue, &event, 0) == pdTRUE;
 }
 
 void EventQueue::reset(){
 	while(uxQueueMessagesWaiting(queue) > 0){
 		Event evt = {};
 		get(evt, 0);
-		free(evt.data);
+		if(!evt.isInline){
+			free(evt.data);
+		}
 	}
 }
 

main/src/Util/Events.h

@@ -13,6 +13,8 @@ enum class Facility { Input, Encoders, Potentiometers, WiFiSTA, TCP, Comm, Rover
 struct Event {
 	Facility facility;
 	void* data;
+	uint8_t inlineData[8];
+	bool isInline;
 };
 
 class EventQueue;
@@ -57,7 +59,7 @@ class EventQueue {
 		bool killPill;
 	};
 
-	bool post(Facility facility, void* data);
+	bool post(Facility facility, const void* data, size_t size);
 	friend Events;
 
 };