diff --git a/main/src/Screens/DriveScreen/DriveScreen.cpp b/main/src/Screens/DriveScreen/DriveScreen.cpp index 1ce947b..6c51cf3 100644 --- a/main/src/Screens/DriveScreen/DriveScreen.cpp +++ b/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(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); } } diff --git a/main/src/Screens/DriveScreen/DriveScreen.h b/main/src/Screens/DriveScreen/DriveScreen.h index 9777ee0..a1fda84 100644 --- a/main/src/Screens/DriveScreen/DriveScreen.h +++ b/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; diff --git a/main/src/Services/Feed.cpp b/main/src/Services/Feed.cpp index 926f1c7..56a3568 100644 --- a/main/src/Services/Feed.cpp +++ b/main/src/Services/Feed.cpp @@ -2,6 +2,7 @@ #include "Util/stdafx.h" #include #include +#include static const char* tag = "Feed"; @@ -34,16 +35,16 @@ bool Feed::nextFrame(std::function freeImgs[3] = { true, true, true }; std::vector frameImgs[3]; std::mutex readyFrameMut; diff --git a/main/src/Services/TCPClient.cpp b/main/src/Services/TCPClient.cpp index 1adadaf..0422c0d 100644 --- a/main/src/Services/TCPClient.cpp +++ b/main/src/Services/TCPClient.cpp @@ -2,6 +2,7 @@ #include "Util/Events.h" #include #include +#include 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(); diff --git a/main/src/Services/UDPListener.cpp b/main/src/Services/UDPListener.cpp index 93b365f..e269485 100644 --- a/main/src/Services/UDPListener.cpp +++ b/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; } diff --git a/main/src/Services/UDPListener.h b/main/src/Services/UDPListener.h index 5fb9a4e..ac4597b 100644 --- a/main/src/Services/UDPListener.h +++ b/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; diff --git a/main/src/Util/Events.cpp b/main/src/Util/Events.cpp index c80c37f..8152a7d 100644 --- a/main/src/Util/Events.cpp +++ b/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 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,19 +47,34 @@ 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; } @@ -75,7 +82,9 @@ void EventQueue::reset(){ while(uxQueueMessagesWaiting(queue) > 0){ Event evt = {}; get(evt, 0); - free(evt.data); + if(!evt.isInline){ + free(evt.data); + } } } diff --git a/main/src/Util/Events.h b/main/src/Util/Events.h index 1939eeb..c334344 100644 --- a/main/src/Util/Events.h +++ b/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; };