add SetFPSCommand and ToggleStreamCommand, cleanup streaming implemen…

…tation, add FPS limiting in TCP stream, improve logging a bit

ESP/lib/src/data/CommandManager/Command.cpp

@@ -39,4 +39,33 @@ CommandResult SetMDNSCommand::execute() {
 CommandResult SaveConfigCommand::execute() {
   projectConfig.save();
   return CommandResult::getSuccessResult("CONFIG SAVED");
+}
+
+CommandResult SetFPSCommand::validate() {
+  if (!data.containsKey("fps") || !data["hostname"])
+    return CommandResult::getErrorResult("Missing fps or FPS were negative");
+
+  return CommandResult::getSuccessResult("");
+}
+
+CommandResult SetFPSCommand::execute() {
+  // handle FPS here, poc of the interface:
+  // auto defaultCameraSettings = projectConfig.getDefaultCameraSettings();
+  // projectConfig.setCamera(..defaultCameraSettings, data["fps"]);
+  return CommandResult::getSuccessResult("FPS set to:" +
+                                         data["fps"].as<std::string>());
+}
+
+CommandResult ToggleStreamCommand::validate() {
+  if (!data.containsKey("state"))
+    return CommandResult::getErrorResult("Missing state field");
+
+  return CommandResult::getSuccessResult("");
+}
+
+CommandResult ToggleStreamCommand::execute() {
+  this->streamServer.toggleTCPStream(data["state"].as<bool>());
+
+  return CommandResult::getSuccessResult("TCP Stream state set to:" +
+                                         data["state"].as<std::string>());
 }

ESP/lib/src/data/CommandManager/Command.hpp

@@ -5,6 +5,7 @@
 #include <string>
 #include <variant>
 #include "data/config/project_config.hpp"
+#include "network/stream/streamServer.hpp"
 
 class CommandResult {
  private:
@@ -91,4 +92,27 @@ class SaveConfigCommand : public ICommand {
   CommandResult execute() override;
 };
 
+class SetFPSCommand : public ICommand {
+  ProjectConfig& projectConfig;
+  JsonVariant data;
+
+ public:
+  SetFPSCommand(ProjectConfig& projectConfig, JsonVariant data)
+      : projectConfig(projectConfig), data(data) {}
+
+  CommandResult validate() override;
+  CommandResult execute() override;
+};
+
+class ToggleStreamCommand : public ICommand {
+  StreamServer& streamServer;
+  JsonVariant data;
+
+ public:
+  ToggleStreamCommand(StreamServer& streamServer, JsonVariant data)
+      : streamServer(streamServer), data(data) {}
+
+  CommandResult validate() override;
+  CommandResult execute() override;
+};
 #endif

ESP/lib/src/data/CommandManager/CommandManager.cpp

@@ -11,6 +11,12 @@ std::unique_ptr<ICommand> CommandManager::createCommand(CommandType commandType,
       return std::make_unique<SetMDNSCommand>(this->projectConfig, data);
     case CommandType::SAVE_CONFIG:
       return std::make_unique<SaveConfigCommand>(this->projectConfig);
+    case CommandType::SET_FPS:
+      return std::make_unique<SetFPSCommand>(this->projectConfig, data);
+    case CommandType::TOGGLE_STREAM:
+      return std::make_unique<ToggleStreamCommand>(this->streamServer, data);
+    default:
+      return nullptr;
   }
 }
 
@@ -130,5 +136,14 @@ CommandManager::createCommandFromJsonVariant(JsonVariant& command) {
   }
 
   auto command_data = command["data"].as<JsonVariant>();
-  return this->createCommand(command_type, command_data);
+  auto command_ptr = this->createCommand(command_type, command_data);
+
+  if (!command_ptr) {
+    std::string error = Helpers::format_string("Command is not supported: %s",
+                                               command["command"]);
+    log_e("%s", error.c_str());
+    return CommandResult::getErrorResult(error);
+  }
+
+  return command_ptr;
 }

ESP/lib/src/data/CommandManager/CommandManager.hpp

@@ -15,6 +15,7 @@
 
 #include "data/CommandManager/Command.hpp"
 #include "data/config/project_config.hpp"
+#include "network/stream/streamServer.hpp"
 
 struct CommandsPayload {
   JsonVariant data;
@@ -25,18 +26,23 @@ enum CommandType {
   PING,
   SET_WIFI,
   SET_MDNS,
+  SET_FPS,
+  TOGGLE_STREAM,
   SAVE_CONFIG,
 };
 
 const std::unordered_map<std::string, CommandType> commandMap = {
     {"ping", CommandType::PING},
     {"set_wifi", CommandType::SET_WIFI},
     {"set_mdns", CommandType::SET_MDNS},
+    {"set_fps", CommandType::SET_FPS},
+    {"toggle_stream", CommandType::TOGGLE_STREAM},
     {"save_config", CommandType::SAVE_CONFIG}};
 
 class CommandManager {
  private:
   ProjectConfig& projectConfig;
+  StreamServer& streamServer;
 
   std::string join_strings(std::vector<std::string> const& strings,
                            std::string delim) {
@@ -60,8 +66,8 @@ class CommandManager {
   //   // TODO rewrite camera handler to be simpler and easier to change
 
  public:
-  CommandManager(ProjectConfig& projectConfig)
-      : projectConfig(projectConfig) {};
+  CommandManager(ProjectConfig& projectConfig, StreamServer& streamServer)
+      : projectConfig(projectConfig), streamServer(streamServer) {};
 
   CommandResult handleSingleCommand(CommandsPayload commandsPayload);
   CommandResult handleBatchCommands(CommandsPayload commandsPayload);

ESP/lib/src/network/stream/streamServer.cpp

@@ -71,18 +71,12 @@ esp_err_t StreamHelpers::stream(httpd_req_t* req) {
   return res;
 }
 
-StreamServer::StreamServer(const int STREAM_PORT)
-    : STREAM_SERVER_PORT(STREAM_PORT) {
-  memcpy(initial_packet_buffer, ETVR_HEADER, sizeof(ETVR_HEADER));
-}
-
 int StreamServer::startStreamServer() {
   httpd_config_t config = HTTPD_DEFAULT_CONFIG();
   config.stack_size = 20480;
   config.max_uri_handlers = 1;
   config.server_port = this->STREAM_SERVER_PORT;
   config.ctrl_port = this->STREAM_SERVER_PORT;
-  config.stack_size = 20480;
 
   httpd_uri_t stream_page = {.uri = "/",
                              .method = HTTP_GET,
@@ -96,90 +90,28 @@ int StreamServer::startStreamServer() {
   else {
     httpd_register_uri_handler(camera_stream, &stream_page);
     Serial.println("Stream server initialized");
+    String serverStatusMessage = "The stream is under: http://";
+
     switch (wifiStateManager.getCurrentState()) {
       case WiFiState_e::WiFiState_ADHOC:
-        Serial.printf("\n\rThe stream is under: http://%s:%i\n\r",
-                      WiFi.softAPIP().toString().c_str(),
-                      this->STREAM_SERVER_PORT);
+        // this should be Serial.printf but for some odd reason
+        // Serial.printf shows up only on debug, not in release
+        Serial.println((serverStatusMessage + WiFi.softAPIP().toString() + ":" +
+                        this->STREAM_SERVER_PORT + "\n\r")
+                           .c_str());
         break;
       default:
-        Serial.printf("\n\rThe stream is under: http://%s:%i\n\r",
-                      WiFi.localIP().toString().c_str(),
-                      this->STREAM_SERVER_PORT);
+        Serial.println((serverStatusMessage + WiFi.localIP().toString() + ":" +
+                        this->STREAM_SERVER_PORT + "\n\r")
+                           .c_str());
         break;
     }
     return 0;
   }
 }
 
-bool StreamServer::startUDPStreamServer() {
-  socket = AsyncUDP();
-  return socket.listen(this->STREAM_SERVER_PORT + 1);
-}
-
-void StreamServer::sendUDPFrame() {
-  ///////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////
-  //   TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
-  //
-  //   ADD PROTOCOL VERSION
-  //
-  //   TODO TODO TODO TODO TODO TODO TODO TODO TODO TODO
-  ///////////////////////////////////////////////////////
-  ///////////////////////////////////////////////////////
-
-  if (!last_frame)
-    last_frame = esp_timer_get_time();
-
-  size_t len = 0;
-  uint8_t* buf = NULL;
-
-  auto fb = esp_camera_fb_get();
-  if (fb) {
-    len = fb->len;
-    buf = fb->buf;
-  } else {
-    log_e("Camera capture failed");
-    return;
-  }
-
-  // we're sending the initial header with the total number of chunks first
-  // we can then later detect new frame with the header packets
-  uint8_t totalChunks = (len + CHUNK_SIZE - 1) / CHUNK_SIZE;
-  initial_packet_buffer[sizeof(ETVR_HEADER)] = totalChunks;
-  socket.broadcastTo(initial_packet_buffer, sizeof(initial_packet_buffer),
-                     this->STREAM_SERVER_PORT);
-
-  for (uint8_t i = 0; i < totalChunks; i++) {
-    auto offset = i * CHUNK_SIZE;
-    // we need to make sure we don't overread
-    auto chunkSize = (offset + CHUNK_SIZE <= len) ? CHUNK_SIZE : len - offset;
-    packet_buffer[0] = static_cast<uint8_t>(i);
-    // since this is a pointer, we can just add an offset to it, with a
-    // chunksize to read and we're done
-    memcpy(packet_buffer + 1, buf + offset, chunkSize);
-    socket.broadcastTo(packet_buffer, chunkSize + 1, this->STREAM_SERVER_PORT);
-  }
-
-  if (fb) {
-    esp_camera_fb_return(fb);
-    fb = NULL;
-    buf = NULL;
-  } else if (buf) {
-    free(buf);
-    buf = NULL;
-  }
-
-  long request_end = millis();
-  long latency = request_end - last_request_time;
-  last_request_time = request_end;
-
-  log_d("Size: %uKB, Time: %ums (%ifps) chunks: %u \n", len / 1024, latency,
-        1000 / latency, totalChunks);
-}
-
 bool StreamServer::startTCPStreamServer() {
-  tcp_server = new AsyncServer(this->STREAM_SERVER_PORT);
+  tcp_server = new AsyncServer(this->TCP_STREAM_SERVER_PORT);
   tcp_server->onClient(
       [this](void* arg, AsyncClient* client) {
         this->handleNewTCPClient(arg, client);
@@ -220,12 +152,16 @@ void StreamServer::handleNewTCPClient(void* arg, AsyncClient* client) {
 
 void StreamServer::sendTCPFrame() {
   if (this->tcp_connected_client == nullptr ||
-      !this->tcp_connected_client->connected()) {
+      !this->tcp_connected_client->connected() || this->pauseTCPStream) {
     return;
   }
 
-  if (!last_frame)
-    last_frame = esp_timer_get_time();
+  // todo test this
+  if (last_time_frame_sent &&
+      last_time_frame_sent - millis() < target_fps_time) {
+    return;
+  }
+  last_time_frame_sent = millis();
 
   auto fb = esp_camera_fb_get();
   if (!fb) {
@@ -234,7 +170,6 @@ void StreamServer::sendTCPFrame() {
   }
 
   size_t len = fb->len;
-
   this->tcp_connected_client->write(ETVR_HEADER_BYTES, 4);
   this->tcp_connected_client->write((const char*)fb->buf, fb->len);
 
@@ -247,4 +182,22 @@ void StreamServer::sendTCPFrame() {
   last_request_time = request_end;
   log_d("Size: %uKB, Time: %ums (%ifps)\n", len / 1024, latency,
         1000 / latency);
+}
+
+std::string StreamServer::getName() {
+  return "StreamServer";
+}
+
+void StreamServer::toggleTCPStream(bool state) {
+  pauseTCPStream = state;
+}
+
+void StreamServer::update(ConfigState_e event) {
+  switch (event) {
+    case ConfigState_e::cameraConfigUpdated:
+      // add FPS update here
+      break;
+    default:
+      break;
+  }
 }

ESP/lib/src/network/stream/streamServer.hpp

@@ -11,6 +11,8 @@
 #include <sstream>
 
 #include "data/StateManager/StateManager.hpp"
+#include "data/config/project_config.hpp"
+#include "data/utilities/Observer.hpp"
 #include "data/utilities/helpers.hpp"
 
 // Camera includes
@@ -26,32 +28,41 @@ namespace StreamHelpers {
   esp_err_t stream(httpd_req_t* req);
 }
 // namespace StreamHelpers
-class StreamServer {
+class StreamServer : public IObserver<ConfigState_e> {
  private:
-  AsyncUDP socket;
+  ProjectConfig& configManager;
+  int64_t last_frame = 0;
+
   AsyncServer* tcp_server;
   AsyncClient* tcp_connected_client;
   httpd_handle_t camera_stream = nullptr;
-  uint8_t initial_packet_buffer[6];
-  uint8_t packet_buffer[CHUNK_SIZE];
 
-  int64_t last_frame = 0;
   long last_request_time = 0;
+  int STREAM_SERVER_PORT = 80;
+  int TCP_STREAM_SERVER_PORT = 82;
+
+  int last_time_frame_sent = 0;
+  float target_fps_time = 1000 / 30;
 
-  int STREAM_SERVER_PORT;
+  bool pauseTCPStream = true;
 
  public:
-  StreamServer(const int STREAM_PORT = 80);
+  StreamServer(ProjectConfig& configManager,
+               const int STREAM_PORT,
+               const int TPC_SERVER_PORT)
+      : configManager(configManager),
+        STREAM_SERVER_PORT(STREAM_PORT),
+        TCP_STREAM_SERVER_PORT(TPC_SERVER_PORT) {};
+
   int startStreamServer();
-  bool startUDPStreamServer();
   bool startTCPStreamServer();
 
-  // rewrite this to an RTOS task pinned to the second core, for testing this is
-  // fine https://randomnerdtutorials.com/esp32-dual-core-arduino-ide/
-  void sendUDPFrame();
-
+  void toggleTCPStream(bool state);
   void sendTCPFrame();
   void handleNewTCPClient(void* arg, AsyncClient* client);
+
+  void update(ConfigState_e event) override;
+  std::string getName() override;
 };
 
 #endif  // STREAM_SERVER_HPP