Performance tuning. Memory management

PlatformIO/README.md

@@ -34,7 +34,18 @@ Open workspace file `esp32-cam-rtos-pio\esp32-cam-rtos-pio.code-workspace` with
 
 Switch to Platform IO menu then build and upload appropriate camera options
 
-If your cemera model is not listed - read on or try contracting me. 
+If your cemera model is not listed - read on or try contacting me to create appropriate build option, device file and partition scheme. 
+
+When firmware is running, connect to the WIFI AP it creates:
+
+- SSID: mjpeg-ap
+- Password: MJPEG123
+
+Provide WIFI credentials for your network
+
+Once connected, the MJPEG stream is available on: Stream Link: http://<your local network IP address>/mjpeg/1
+
+e.g.: Stream Link: http://192.168.122.132/mjpeg/1
 
 
 

PlatformIO/esp32-cam-rtos-pio/include/streaming.h

@@ -14,7 +14,7 @@ typedef struct {
 
 typedef struct {
   uint8_t   cnt;  // served to clients counter. when equal to number of active clients, could be deleted
-  uint32_t* nxt;  // next chunck
+  void*     nxt;  // next chunck
   uint32_t  fnm;  // frame number
   uint32_t  siz;  // frame size
   uint8_t*  dat;  // frame pointer
@@ -25,7 +25,12 @@ void camCB(void* pvParameters);
 void handleJPGSstream(void);
 void streamCB(void * pvParameters);
 void mjpegCB(void * pvParameters);
-char* allocateMemory(char* aPtr, size_t aSize, bool psramOnly = false);
+
+#define FAIL_IF_OOM true
+#define OK_IF_OOM   false
+#define PSRAM_ONLY  true
+#define ANY_MEMORY  false
+char* allocateMemory(char* aPtr, size_t aSize, bool fail = FAIL_IF_OOM, bool psramOnly = ANY_MEMORY);
 
 extern const char* HEADER;
 extern const char* BOUNDARY;

PlatformIO/esp32-cam-rtos-pio/lib/ArduinoLog/src/ArduinoLog.h

@@ -56,6 +56,8 @@ typedef void (*printfunction)(Print*);
    ---- Wildcards
 
    %s   replace with an string (char*)
+   %S   replace with a String (String)
+   %I   replace with an IP address
    %c   replace with an character
    %d   replace with an integer value
    %l   replace with an long value

PlatformIO/esp32-cam-rtos-pio/lib/AverageFilter/LICENSE.txt

@@ -0,0 +1,27 @@
+Copyright (c) 2015-2020, Anatoli Arkhipenko.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, 
+   this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+   may be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
+IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
+OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

PlatformIO/esp32-cam-rtos-pio/lib/AverageFilter/README.md

@@ -0,0 +1,16 @@
+# Average Filter
+
+### Digital implementation of an average filter
+[![arduino-library-badge](https://www.ardu-badge.com/badge/AverageFilter.svg?)](https://www.ardu-badge.com/AverageFilter)
+
+##### With variable number of tracked values and externally provided storage
+
+##### Version 1.0.0
+
+
+
+###### Changelog:
+
+v1.0.0:
+
+- 2015-11-18 - initial release

PlatformIO/esp32-cam-rtos-pio/lib/AverageFilter/keywords.txt

@@ -0,0 +1,23 @@
+#######################################
+# Syntax Coloring Map For TaskManager
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+averageFilter	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+initialize	KEYWORD2
+value	KEYWORD2
+currentValue	KEYWORD2
+samples	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+

PlatformIO/esp32-cam-rtos-pio/lib/AverageFilter/library.json

@@ -0,0 +1,22 @@
+{
+  "name": "AverageFilter",
+  "keywords": "integer, filter, average, signal, processing",
+  "description": "Integer implementation of the Average Filter",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/arkhipenko/AverageFilter"
+  },
+  "authors":
+  [
+    {
+      "name": "Anatoli Arkhipenko",
+      "email": "arkhipenko@hotmail.com",
+      "url": "https://github.com/arkhipenko",
+      "maintainer": true
+    }
+  ],
+  "version": "1.0.1",
+  "frameworks": "arduino",
+  "platforms": "*"
+}

PlatformIO/esp32-cam-rtos-pio/lib/AverageFilter/src/AverageFilter.h

@@ -0,0 +1,97 @@
+//
+// Digital implementation of an average filter
+// with variable number of samples
+//
+
+#include <Arduino.h>
+
+#ifndef _AVERAGEFILTER_H
+#define _AVERAGEFILTER_H
+
+template<typename T>
+class averageFilter {
+    public:
+        averageFilter(int aSamples);
+        averageFilter(int aSamples, T* aStorage);
+        ~averageFilter();
+        
+        void    initialize();
+        T       value(T aSample);
+        void    setSamples( int aSamples );
+
+        inline  T         currentValue() { return iCV; }
+        inline  int32_t   samples() { return iCount; }
+        inline  bool      memoryError() { return iReadings == NULL; }
+        
+    private:
+        T         *iReadings;
+        T         iTotal;
+        T         iCV;
+        int32_t   iIndex;
+        int32_t   iCount;
+        int32_t   iSamples;
+};
+
+
+
+template<typename T>
+averageFilter<T>::averageFilter(int aSamples) {
+    iSamples = constrain(aSamples, 1, aSamples); 
+    iReadings = (T *) malloc (sizeof (T) * aSamples);
+// if there is a memory allocation error.
+   if (iReadings == NULL) {
+    iReadings = &iCV;
+    iSamples = 1;
+   }
+  initialize();
+}
+
+template<typename T>
+averageFilter<T>::averageFilter(int aSamples, T* aStorage) {
+    iSamples = constrain(aSamples, 1, aSamples); 
+    iReadings = aStorage;
+// if storage provided is NULL
+   if (iReadings == NULL) {
+    iReadings = &iCV;
+    iSamples = 1;
+   }
+  initialize();
+}
+
+template<typename T>
+void averageFilter<T>::setSamples(int aSamples) {
+    iSamples = constrain(aSamples, 1, aSamples); 
+    if ( iCount ) initialize();
+}
+
+template<typename T>
+averageFilter<T>::~averageFilter() {
+    if ( iReadings && iReadings != &iCV ) free (iReadings);
+    iReadings = NULL;
+    iIndex = 0;
+    iTotal = 0;
+    iCount = 0;
+}
+
+template<typename T>
+void averageFilter<T>::initialize() {
+    iIndex = 0;
+    iTotal = 0;
+    iCount = 0;
+    iCV = 0;
+    for (int i=0; i<iSamples; i++) iReadings[i]=0;
+}
+
+template<typename T>
+T   averageFilter<T>::value(T aSample) {
+    // if ( !iReadings ) return 0;
+    iTotal -= iReadings[iIndex];
+    iReadings[iIndex] = aSample;
+    iTotal += aSample;
+    if (++iIndex >= iSamples) iIndex = 0;
+    if (++iCount > iSamples) iCount = iSamples;
+    iCV = (T) iTotal/iCount;
+    return iCV;
+}
+
+#endif  // _AVERAGEFILTER_H

PlatformIO/esp32-cam-rtos-pio/platformio.ini

@@ -9,7 +9,7 @@
 ; https://docs.platformio.org/page/projectconf.html
 
 [platformio]
-	default_envs = esp-eye-debug
+	default_envs = ai-thinker-cam-debug ;esp-eye-debug
 	boards_dir = ./boards
 	src_dir = src
 	lib_dir = lib
@@ -76,14 +76,46 @@
     ${env.build_flags}
     -D CAMERA_MODEL_AI_THINKER
     -D FRAME_SIZE=FRAMESIZE_HVGA
-    -D FPS=5
-    -D WSINTERVAL=40
+    -D FPS=10
+    -D WSINTERVAL=100
     -D MAX_CLIENTS=10
-    -D JPEG_QUALITY=24 ; 0-63 lower means higher quality
+    -D JPEG_QUALITY=16 ; 0-63 lower means higher quality
     -D LOG_LEVEL=0
     -D DISABLE_LOGGING
     -D WM_NODEBUG
 
+
+[env:ai-thinker-cam-debug]
+; ESP32-CAM AI-Thinker
+; https://docs.platformio.org/en/stable/boards/espressif32/esp32cam.html'
+; https://docs.ai-thinker.com/en/esp32
+; board has 4MB PSRAM
+  build_type = debug
+  platform = espressif32
+  framework = arduino, espidf
+  board = ai-thinker-cam
+  board_build.partitions = ai-thinker-cam.csv 
+
+  lib_deps = 
+    ${env.lib_deps}
+
+  lib_ldf_mode = deep+
+
+	upload_speed = 921600
+	monitor_speed = 115200
+
+	build_flags = 
+    ${env.build_flags}
+    -D CAMERA_MODEL_AI_THINKER
+    -D FRAME_SIZE=FRAMESIZE_HVGA
+    -D FPS=10
+    -D WSINTERVAL=100
+    -D MAX_CLIENTS=10
+    -D JPEG_QUALITY=16 ; 0-63 lower means higher quality
+    -D LOG_LEVEL=6
+    -D BENCHMARK
+
+
 [env:esp-eye]
 ; Espressif ESP-EYE
 ; https://github.com/espressif/esp-who/blob/master/docs/en/get-started/ESP-EYE_Getting_Started_Guide.md
@@ -108,10 +140,10 @@
     -D CAMERA_MODEL_ESP_EYE
     ; -D FLIP_VERTICALLY
     -D FRAME_SIZE=FRAMESIZE_VGA
-    -D FPS=15
-    -D WSINTERVAL=40
+    -D FPS=10
+    -D WSINTERVAL=100
     -D MAX_CLIENTS=10
-    -D JPEG_QUALITY=16 ; 0-63 lower means higher quality
+    -D JPEG_QUALITY=24 ; 0-63 lower means higher quality
     -D LOG_LEVEL=0
     -D DISABLE_LOGGING
     -D WM_NODEBUG
@@ -140,14 +172,14 @@
     ${env.build_flags}
     -D CAMERA_MODEL_ESP_EYE
     -D FLIP_VERTICALLY
-    -D FRAME_SIZE=FRAMESIZE_VGA
-    -D FPS=15
-    -D WSINTERVAL=40
+    -D FRAME_SIZE=FRAMESIZE_SVGA
+    -D FPS=10
+    -D WSINTERVAL=100
     -D MAX_CLIENTS=10
-    -D JPEG_QUALITY=16 ; 0-63 lower means higher quality
+    -D JPEG_QUALITY=32 ; 0-63 lower means higher quality
     -D LOG_LEVEL=6
     -D WM_DEBUG_LEVEL=WM_DEBUG_VERBOSE
-
+    -D BENCHMARK
 
 
 ; EXAMPLE of the additional board configuration
@@ -179,7 +211,7 @@
     ; -D FLIP_VERTICALLY
     -D FRAME_SIZE=FRAMESIZE_HVGA
     -D FPS=5
-    -D WSINTERVAL=40
+    -D WSINTERVAL=100
     -D MAX_CLIENTS=10
     -D JPEG_QUALITY=24 ; 0-63 lower means higher quality
     -D LOG_LEVEL=0

PlatformIO/esp32-cam-rtos-pio/sdkconfig.esp-eye

@@ -50,8 +50,16 @@ CONFIG_BOOTLOADER_LOG_LEVEL_NONE=y
 # CONFIG_BOOTLOADER_LOG_LEVEL_DEBUG is not set
 # CONFIG_BOOTLOADER_LOG_LEVEL_VERBOSE is not set
 CONFIG_BOOTLOADER_LOG_LEVEL=0
+
+#
+# Serial Flash Configurations
+#
 # CONFIG_BOOTLOADER_SPI_CUSTOM_WP_PIN is not set
 CONFIG_BOOTLOADER_SPI_WP_PIN=7
+# CONFIG_BOOTLOADER_FLASH_DC_AWARE is not set
+CONFIG_BOOTLOADER_FLASH_XMC_SUPPORT=y
+# end of Serial Flash Configurations
+
 CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y
 # CONFIG_BOOTLOADER_FACTORY_RESET is not set
 # CONFIG_BOOTLOADER_APP_TEST is not set
@@ -65,7 +73,6 @@ CONFIG_BOOTLOADER_SKIP_VALIDATE_IN_DEEP_SLEEP=y
 # CONFIG_BOOTLOADER_SKIP_VALIDATE_ALWAYS is not set
 CONFIG_BOOTLOADER_RESERVE_RTC_SIZE=0x10
 # CONFIG_BOOTLOADER_CUSTOM_RESERVE_RTC is not set
-CONFIG_BOOTLOADER_FLASH_XMC_SUPPORT=y
 # end of Bootloader config
 
 #
@@ -581,6 +588,7 @@ CONFIG_ESP_SLEEP_RTC_BUS_ISO_WORKAROUND=y
 CONFIG_ESP_SLEEP_PSRAM_LEAKAGE_WORKAROUND=y
 CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=y
 # CONFIG_ESP_SLEEP_MSPI_NEED_ALL_IO_PU is not set
+CONFIG_ESP_SLEEP_GPIO_ENABLE_INTERNAL_RESISTORS=y
 # end of Sleep Config
 
 #
@@ -601,6 +609,10 @@ CONFIG_ESP_IPC_ISR_ENABLE=y
 # LCD and Touch Panel
 #
 
+#
+# LCD Touch Drivers are maintained in the IDF Component Registry
+#
+
 #
 # LCD Peripheral Configuration
 #
@@ -710,6 +722,10 @@ CONFIG_ESP32_WIFI_STATIC_TX_BUFFER=y
 CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=0
 CONFIG_ESP32_WIFI_STATIC_TX_BUFFER_NUM=16
 CONFIG_ESP32_WIFI_CACHE_TX_BUFFER_NUM=32
+CONFIG_ESP_WIFI_STATIC_RX_MGMT_BUFFER=y
+# CONFIG_ESP_WIFI_DYNAMIC_RX_MGMT_BUFFER is not set
+CONFIG_ESP_WIFI_DYNAMIC_RX_MGMT_BUF=0
+CONFIG_ESP_WIFI_RX_MGMT_BUF_NUM_DEF=5
 # CONFIG_ESP32_WIFI_CSI_ENABLED is not set
 CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y
 CONFIG_ESP32_WIFI_TX_BA_WIN=6
@@ -905,6 +921,7 @@ CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y
 #
 CONFIG_LWIP_LOCAL_HOSTNAME="espressif"
 # CONFIG_LWIP_NETIF_API is not set
+CONFIG_LWIP_TCPIP_TASK_PRIO=18
 # CONFIG_LWIP_TCPIP_CORE_LOCKING is not set
 # CONFIG_LWIP_CHECK_THREAD_SAFETY is not set
 CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y
@@ -918,6 +935,7 @@ CONFIG_LWIP_SO_REUSE=y
 CONFIG_LWIP_SO_REUSE_RXTOALL=y
 CONFIG_LWIP_SO_RCVBUF=y
 # CONFIG_LWIP_NETBUF_RECVINFO is not set
+CONFIG_LWIP_IP_DEFAULT_TTL=64
 CONFIG_LWIP_IP4_FRAG=y
 CONFIG_LWIP_IP6_FRAG=y
 # CONFIG_LWIP_IP4_REASSEMBLY is not set
@@ -970,6 +988,8 @@ CONFIG_LWIP_TCP_SND_BUF_DEFAULT=5744
 CONFIG_LWIP_TCP_WND_DEFAULT=5744
 CONFIG_LWIP_TCP_RECVMBOX_SIZE=6
 CONFIG_LWIP_TCP_QUEUE_OOSEQ=y
+CONFIG_LWIP_TCP_OOSEQ_TIMEOUT=6
+CONFIG_LWIP_TCP_OOSEQ_MAX_PBUFS=0
 # CONFIG_LWIP_TCP_SACK_OUT is not set
 # CONFIG_LWIP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set
 CONFIG_LWIP_TCP_OVERSIZE_MSS=y
@@ -1026,6 +1046,13 @@ CONFIG_LWIP_SNTP_MAX_SERVERS=1
 CONFIG_LWIP_SNTP_UPDATE_DELAY=3600000
 # end of SNTP
 
+#
+# DNS
+#
+CONFIG_LWIP_DNS_MAX_SERVERS=3
+# CONFIG_LWIP_FALLBACK_DNS_SERVER_SUPPORT is not set
+# end of DNS
+
 #
 # Hooks
 #
@@ -1254,6 +1281,20 @@ CONFIG_PTHREAD_TASK_CORE_DEFAULT=-1
 CONFIG_PTHREAD_TASK_NAME_DEFAULT="pthread"
 # end of PThreads
 
+#
+# Main Flash configuration
+#
+
+#
+# Optional and Experimental Features (READ DOCS FIRST)
+#
+
+#
+# Features here require specific hardware (READ DOCS FIRST!)
+#
+# end of Optional and Experimental Features (READ DOCS FIRST)
+# end of Main Flash configuration
+
 #
 # SPI Flash driver
 #

PlatformIO/esp32-cam-rtos-pio/sdkconfig.esp-eye-debug

@@ -39,8 +39,8 @@ CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16
 # Bootloader config
 #
 CONFIG_BOOTLOADER_OFFSET_IN_FLASH=0x1000
-# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_SIZE is not set
-CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_DEBUG=y
+CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_SIZE=y
+# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_DEBUG is not set
 # CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_PERF is not set
 # CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_NONE is not set
 # CONFIG_BOOTLOADER_LOG_LEVEL_NONE is not set

PlatformIO/esp32-cam-rtos-pio/src/main.cpp

@@ -81,7 +81,10 @@ void setup()
   setupLogging();
 
   Log.trace("\n\nMulti-client MJPEG Server\n");
+  Log.trace("setup: total heap : %d\n", ESP.getHeapSize());
   Log.trace("setup: free heap  : %d\n", ESP.getFreeHeap());
+  Log.trace("setup: free psram : %d\n", ESP.getPsramSize());
+  Log.trace("setup: free psram : %d\n", ESP.getFreePsram());
 
   static camera_config_t camera_config = {
     .pin_pwdn       = PWDN_GPIO_NUM,
@@ -101,13 +104,14 @@ void setup()
     .pin_href       = HREF_GPIO_NUM,
     .pin_pclk       = PCLK_GPIO_NUM,
 
+    // .xclk_freq_hz   = 16000000,
     .xclk_freq_hz   = 20000000,
     .ledc_timer     = LEDC_TIMER_0,
     .ledc_channel   = LEDC_CHANNEL_0,
     .pixel_format   = PIXFORMAT_JPEG,
     .frame_size     = FRAME_SIZE,
     .jpeg_quality   = JPEG_QUALITY,
-    .fb_count       = 2,
+    .fb_count       = 1, //2,
     .fb_location = CAMERA_FB_IN_DRAM,
     .grab_mode = CAMERA_GRAB_LATEST,
     // .sccb_i2c_port = -1
@@ -166,7 +170,7 @@ void setup()
     NULL,
     2,
     &tMjpeg,
-    APP_CPU);
+    PRO_CPU);
 
   Log.trace("setup complete: free heap  : %d\n", ESP.getFreeHeap());
 }

PlatformIO/esp32-cam-rtos-pio/src/streaming.cpp

@@ -10,9 +10,8 @@ const int bdrLen = strlen(BOUNDARY);
 const int cntLen = strlen(CTNTTYPE);
 volatile uint32_t frameNumber;
 
-
-frameChunck_t* fstFrame;  // first frame
-frameChunck_t* curFrame;  // current frame being captured by the camera
+frameChunck_t* fstFrame = NULL;  // first frame
+frameChunck_t* curFrame = NULL;  // current frame being captured by the camera
 
 void mjpegCB(void* pvParameters) {
   TickType_t xLastWakeTime;
@@ -49,16 +48,19 @@ void mjpegCB(void* pvParameters) {
     server.handleClient();
 
     //  After every server client handling request, we let other tasks run and then pause
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
   }
 }
 
 
 // ==== Memory allocator that takes advantage of PSRAM if present =======================
-char* allocateMemory(char* aPtr, size_t aSize, bool psramOnly) {
+char* allocateMemory(char* aPtr, size_t aSize, bool fail, bool psramOnly) {
 
   //  Since current buffer is too smal, free it
-  if (aPtr != NULL) free(aPtr);
+  if (aPtr != NULL) {
+    free(aPtr);
+    aPtr = NULL;
+  }
 
   char* ptr = NULL;
 
@@ -85,7 +87,7 @@ char* allocateMemory(char* aPtr, size_t aSize, bool psramOnly) {
     }
   }
   // Finally, if the memory pointer is NULL, we were not able to allocate any memory, and that is a terminal condition.
-  if (ptr == NULL) {
+  if (fail && ptr == NULL) {
     Log.fatal("allocateMemory: Out of memory!");
     delay(5000);
     ESP.restart();

PlatformIO/esp32-cam-rtos-pio/src/streaming_all_frames.cpp

@@ -2,6 +2,10 @@
 
 #if defined (CAMERA_ALL_FRAMES)
 
+#if defined(BENCHMARK)
+#include <AverageFilter.h>
+#define BENCHMARK_PRINT_INT 1000
+#endif
 
 // ==== RTOS task to grab frames from the camera =========================
 void camCB(void* pvParameters) {
@@ -14,20 +18,36 @@ void camCB(void* pvParameters) {
   frameNumber = 0;
   xLastWakeTime = xTaskGetTickCount();
 
-  for (;;) {
+#if defined(BENCHMARK)
+    averageFilter<uint32_t> captureAvg(10);
+    averageFilter<uint32_t> tickAvg(10);
+    captureAvg.initialize();
+    tickAvg.initialize();
+    uint32_t lastPrintCam = millis();
+    uint32_t benchmarkStart;
+    uint32_t lastTick = millis();
+#endif
+
   camera_fb_t* fb = NULL;
 
+  for (;;) {
+
+#if defined(BENCHMARK)
+    benchmarkStart = micros();
+#endif
+
     //  Grab a frame from the camera and allocate frame chunk for it
     fb = esp_camera_fb_get();
-    frameChunck_t* f = (frameChunck_t*) allocateMemory(NULL, sizeof(frameChunck_t), true);
+    if ( fb ) {
+      frameChunck_t* f = (frameChunck_t*) allocateMemory(NULL, sizeof(frameChunck_t), OK_IF_OOM, PSRAM_ONLY);
       if ( f ) {
         // char* d = (char*) ps_malloc( fb->len );
-      char* d = (char*) allocateMemory(NULL, fb->len, true); 
+        char* d = (char*) allocateMemory(NULL, fb->len, OK_IF_OOM, PSRAM_ONLY); 
         if ( d == NULL ) {
           free (f);
         }
         else {
-        if ( frameNumber == 0 ) {
+          if ( fstFrame == NULL ) {
             fstFrame = f;
           }
           f->dat = (uint8_t*) d;
@@ -44,25 +64,54 @@ void camCB(void* pvParameters) {
           frameNumber++;
         }
       }
+      else {
+        Log.error("camCB: error allocating memory for frame %d - OOM\n", frameNumber);
+        vTaskDelay(1000);
+      }
       esp_camera_fb_return(fb);
+    }
+    else {
+      Log.error("camCB: error capturing image for frame %d\n", frameNumber);
+      vTaskDelay(1000);
+    }
 
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+#if defined(BENCHMARK)
+    captureAvg.value(micros()-benchmarkStart);
+#endif
+
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
+
+#if defined(BENCHMARK)
+    tickAvg.value(millis()-lastTick);
+    lastTick = millis();
+#endif
 
     if ( noActiveClients == 0 ) {
       // we need to drain the cache if there are no more clients connected
       Log.trace("mjpegCB: All clients disconneted\n");
-      while ( fstFrame->nxt ) {
+      while ( fstFrame !=NULL && fstFrame->nxt ) {
         frameChunck_t* f = (frameChunck_t*) fstFrame->nxt;
         free ( fstFrame->dat );
         free ( fstFrame );
         fstFrame = f;
       }
+      fstFrame = NULL;
+      curFrame = NULL;
       Log.verbose("mjpegCB: free heap           : %d\n", ESP.getFreeHeap());
-      Log.verbose("mjpegCB: min free heap)      : %d\n", ESP.getMinFreeHeap());
+      Log.verbose("mjpegCB: min free heap       : %d\n", ESP.getMinFreeHeap());
       Log.verbose("mjpegCB: max alloc free heap : %d\n", ESP.getMaxAllocHeap());
+      Log.verbose("mjpegCB: free psram          : %d\n", ESP.getFreePsram());
       Log.verbose("mjpegCB: tCam stack wtrmark  : %d\n", uxTaskGetStackHighWaterMark(tCam));
       vTaskSuspend(NULL);  // passing NULL means "suspend yourself"
     }
+
+#if defined(BENCHMARK)
+    if ( millis() - lastPrintCam > BENCHMARK_PRINT_INT ) {
+      lastPrintCam = millis();
+      Log.verbose("mjpegCB: average frame capture time: %d us (tick avg=%d)\n", captureAvg.currentValue(), tickAvg.currentValue() );
+    }
+#endif
+
   }
 }
 
@@ -74,7 +123,17 @@ void handleJPGSstream(void)
   Log.verbose("handleJPGSstream start: free heap  : %d\n", ESP.getFreeHeap());
 
   streamInfo_t* info = (streamInfo_t*) malloc( sizeof(streamInfo_t) );
+  if ( info == NULL ) {
+    Log.error("handleJPGSstream: cannot allocate stream info - OOM\n");
+    return;
+  }
   info->client = new WiFiClient;
+  if ( info->client == NULL ) {
+    Log.error("handleJPGSstream: cannot allocate WiFi client for streaming - OOM\n");
+    free(info);
+    return;
+  }
+
   *(info->client) = server.client();
 
   //  Creating task to push the stream to all connected clients
@@ -92,7 +151,9 @@ void handleJPGSstream(void)
     delete info;
   }
 
+  xSemaphoreTake( frameSync, portMAX_DELAY );
   noActiveClients++;
+  xSemaphoreGive( frameSync );
 
   // Wake up streaming tasks, if they were previously suspended:
   if ( eTaskGetState( tCam ) == eSuspended ) vTaskResume( tCam );
@@ -104,16 +165,11 @@ void streamCB(void * pvParameters) {
   char buf[16];
   TickType_t xLastWakeTime;
   TickType_t xFrequency;
+
   frameChunck_t* myFrame = fstFrame;
-  portMUX_TYPE xSemaphore = portMUX_INITIALIZER_UNLOCKED;
 
   streamInfo_t* info = (streamInfo_t*) pvParameters;
 
-  if ( info == NULL ) {
-    Log.fatal("streamCB: a NULL pointer passed\n");
-    delay(5000);
-    ESP.restart();
-  }
   //  Immediately send this client a header
   info->client->write(HEADER, hdrLen);
   info->client->write(BOUNDARY, bdrLen);
@@ -122,53 +178,95 @@ void streamCB(void * pvParameters) {
   xFrequency = pdMS_TO_TICKS(1000 / FPS);
 
   Log.trace("streamCB: Client connected\n");
+
+#if defined(BENCHMARK)
+  averageFilter<int32_t> streamAvg(10);
+  averageFilter<int32_t> waitAvg(10);
+  averageFilter<uint32_t> frameAvg(10);
+  averageFilter<float> fpsAvg(10);
+  uint32_t streamStart = 0;
+  streamAvg.initialize();
+  waitAvg.initialize();
+  frameAvg.initialize();
+  fpsAvg.initialize();
+  uint32_t lastPrint = millis();
+  uint32_t lastFrame = millis();
+#endif
+
   for (;;) {
-    //  Only bother to send anything if there is someone watching
-    if ( info->client->connected() ) {
     if ( myFrame ) {
 
+#if defined (BENCHMARK)
+        frameAvg.value( myFrame->siz );
+        streamStart = micros();
+#endif        
+
+      if ( info->client->connected() ) {
+        sprintf(buf, "%d\r\n\r\n", myFrame->siz);
         info->client->write(CTNTTYPE, cntLen);
-        sprintf(buf, "%d\r\n\r\n", fstFrame->siz);
         info->client->write(buf, strlen(buf));
-        info->client->write((char*) fstFrame->dat, (size_t)fstFrame->siz);
+        info->client->write((char*) myFrame->dat, (size_t)myFrame->siz);
         info->client->write(BOUNDARY, bdrLen);
-
         // Log.verbose("streamCB: Served frame# %d\n", fstFrame->fnm);
+      }
 
-        if ( myFrame->nxt ) {
-          frameChunck_t* f;
-          f = (frameChunck_t*) myFrame->nxt;
+#if defined (BENCHMARK)
+        streamAvg.value(micros()-streamStart);
+        fpsAvg.value(1000.0 / (float) (millis()-lastFrame) );
+        lastFrame = millis();
+        streamStart = micros();
+#endif
 
-          portENTER_CRITICAL(&xSemaphore);
-          if ( ++myFrame->cnt == noActiveClients ) {
-            assert(myFrame == fstFrame);
+      xSemaphoreTake( frameSync, portMAX_DELAY );
+
+#if defined (BENCHMARK)
+        waitAvg.value(micros()-streamStart);
+#endif
+
+      myFrame->cnt++;
+      frameChunck_t* myNextFrame = (frameChunck_t*) myFrame->nxt; // this maybe NULL - that's ok
+
+      // if serving first frame in the chain, check if we are the last serving task and it needs to be deleted
+      if ( myFrame == fstFrame && fstFrame->cnt >= noActiveClients ) {
         free ( fstFrame->dat );
         fstFrame->dat = NULL;
         free ( fstFrame );
-            fstFrame = f;
-          }
-          portEXIT_CRITICAL(&xSemaphore);
-          myFrame = f;
+        fstFrame = myNextFrame;
       }
+      xSemaphoreGive( frameSync );
+
+      myFrame = myNextFrame;
     }
     else {
       myFrame = fstFrame;
     }
-    }
-    else {
+
+    if ( !info->client->connected() ) {
       //  client disconnected - clean up.
+      xSemaphoreTake( frameSync, portMAX_DELAY );
       noActiveClients--;
-      Log.trace("streamCB: Client disconnected\n");
+      xSemaphoreGive( frameSync );
+
       Log.verbose("streamCB: Stream Task stack wtrmark  : %d\n", uxTaskGetStackHighWaterMark(info->task));
       info->client->stop();
       delete info->client;
       info->client = NULL;
       free( info );
       info = NULL;
+      Log.trace("streamCB: Client disconnected\n");
+      vTaskDelay(100);
       vTaskDelete(NULL);
     }
     //  Let other tasks run after serving every client
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
+
+#if defined (BENCHMARK)
+    if ( millis() - lastPrint > BENCHMARK_PRINT_INT ) {
+      lastPrint = millis();
+      Log.verbose("streamCB: wait avg=%d, stream avg=%d us, frame avg size=%d bytes, fps=%S\n", waitAvg.currentValue(), streamAvg.currentValue(), frameAvg.currentValue(), String(fpsAvg.currentValue()));
+      if ( fstFrame ) Log.verbose("streamCB: current frame: %d, first frame:%d\n", curFrame->fnm, fstFrame->fnm);
+    }
+#endif
   }
 }
 

PlatformIO/esp32-cam-rtos-pio/src/streaming_multiclient_queue.cpp

@@ -1,7 +1,15 @@
 #include "streaming.h"
+#include "lwip/sockets.h"
 
 #if defined(CAMERA_MULTICLIENT_QUEUE)
 
+#if defined(BENCHMARK)
+#include <AverageFilter.h>
+#define BENCHMARK_PRINT_INT 1000
+averageFilter<uint32_t> captureAvg(10);
+uint32_t lastPrintCam = millis();
+#endif
+
 QueueHandle_t streamingClients;
 
 volatile size_t camSize;    // size of the current frame, byte
@@ -14,9 +22,6 @@ void camCB(void* pvParameters) {
   //  A running interval associated with currently desired frame rate
   const TickType_t xFrequency = pdMS_TO_TICKS(1000 / FPS);
 
-  // Mutex for the critical section of swithing the active frames around
-  portMUX_TYPE xSemaphore = portMUX_INITIALIZER_UNLOCKED;
-
   // Creating a queue to track all connected clients
   streamingClients = xQueueCreate( 10, sizeof(WiFiClient*) );
 
@@ -29,8 +34,8 @@ void camCB(void* pvParameters) {
     NULL, //(void*) handler,
     2,
     &tStream,
-    //    APP_CPU);
-    PRO_CPU);
+    APP_CPU);
+    // PRO_CPU);
 
   //  Pointers to the 2 frames, their respective sizes and index of the current frame
   char* fbs[2] = { NULL, NULL };
@@ -40,9 +45,18 @@ void camCB(void* pvParameters) {
   //=== loop() section  ===================
   xLastWakeTime = xTaskGetTickCount();
 
+#if defined(BENCHMARK)
+  captureAvg.initialize();
+#endif
+
+  camera_fb_t* fb = NULL;
+
   for (;;) {
     //  Grab a frame from the camera and query its size
-    camera_fb_t* fb = NULL;
+
+#if defined(BENCHMARK)
+    uint32_t captureStart = micros();
+#endif
 
     fb = esp_camera_fb_get();
     size_t s = fb->len;
@@ -50,7 +64,7 @@ void camCB(void* pvParameters) {
     //  If frame size is more that we have previously allocated - request  125% of the current frame space
     if (s > fSize[ifb]) {
       fSize[ifb] = s * 4 / 3;
-      fbs[ifb] = allocateMemory(fbs[ifb], fSize[ifb]);
+      fbs[ifb] = allocateMemory(fbs[ifb], fSize[ifb], FAIL_IF_OOM, ANY_MEMORY);
     }
 
     //  Copy current frame into local buffer
@@ -58,20 +72,21 @@ void camCB(void* pvParameters) {
     memcpy(fbs[ifb], b, s);
     esp_camera_fb_return(fb);
   
-    //  Let other tasks run and wait until the end of the current frame rate interval (if any time left)
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+#if defined(BENCHMARK)
+    captureAvg.value(micros()-captureStart);
+#endif
 
     //  Only switch frames around if no frame is currently being streamed to a client
     //  Wait on a semaphore until client operation completes
     xSemaphoreTake( frameSync, portMAX_DELAY );
 
     //  Do not allow interrupts while switching the current frame
-    taskENTER_CRITICAL(&xSemaphore);
+    // taskENTER_CRITICAL(&xSemaphore);
     camBuf = fbs[ifb];
     camSize = s;
     ++ifb;
     ifb = ifb & 1;  // this should produce 1, 0, 1, 0, 1 ... sequence
-    taskEXIT_CRITICAL(&xSemaphore);
+    // taskEXIT_CRITICAL(&xSemaphore);
 
     //  Let anyone waiting for a frame know that the frame is ready
     xSemaphoreGive( frameSync );
@@ -80,8 +95,9 @@ void camCB(void* pvParameters) {
     //  and it could start sending frames to the clients, if any
     xTaskNotifyGive( tStream );
 
-    //  Immediately let other (streaming) tasks run
-    taskYIELD();
+
+    //  Let other tasks run and wait until the end of the current frame rate interval (if any time left)
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
 
     //  If streaming task has suspended itself (no active clients to stream to)
     //  there is no need to grab frames from the camera. We can save some juice
@@ -89,6 +105,14 @@ void camCB(void* pvParameters) {
     if ( eTaskGetState( tStream ) == eSuspended ) {
       vTaskSuspend(NULL);  // passing NULL means "suspend yourself"
     }
+
+#if defined(BENCHMARK)
+    if ( millis() - lastPrintCam > BENCHMARK_PRINT_INT ) {
+      lastPrintCam = millis();
+      Log.verbose("mjpegCB: average frame capture time: %d microseconds\n", captureAvg.currentValue() );
+    }
+#endif
+
   }
 }
 
@@ -97,13 +121,21 @@ void camCB(void* pvParameters) {
 void handleJPGSstream(void)
 {
   //  Can only acommodate 10 clients. The limit is a default for WiFi connections
-  if ( !uxQueueSpacesAvailable(streamingClients) ) return;
+  if ( !uxQueueSpacesAvailable(streamingClients) ) {
+    Log.error("handleJPGSstream: Max number of WiFi clients reached\n");
+    return;
+  }
 
   //  Create a new WiFi Client object to keep track of this one
   WiFiClient* client = new WiFiClient();
+  if ( client == NULL ) {
+    Log.error("handleJPGSstream: Can not create new WiFi client - OOM\n");
+    return;
+  }
   *client = server.client();
 
   //  Immediately send this client a header
+  client->setTimeout(1);
   client->write(HEADER, hdrLen);
   client->write(BOUNDARY, bdrLen);
 
@@ -129,6 +161,19 @@ void streamCB(void * pvParameters) {
   ulTaskNotifyTake( pdTRUE,          /* Clear the notification value before exiting. */
                     portMAX_DELAY ); /* Block indefinitely. */
 
+#if defined(BENCHMARK)
+  averageFilter<int32_t> streamAvg(10);
+  averageFilter<int32_t> waitAvg(10);
+  averageFilter<uint32_t> frameAvg(10);
+  averageFilter<float> fpsAvg(10);
+  uint32_t streamStart = 0;
+  streamAvg.initialize();
+  waitAvg.initialize();
+  frameAvg.initialize();
+  uint32_t lastPrint = millis();
+  uint32_t lastFrame = millis();
+#endif
+
   xLastWakeTime = xTaskGetTickCount();
   for (;;) {
     // Default assumption we are running according to the FPS
@@ -157,21 +202,39 @@ void streamCB(void * pvParameters) {
           //  Ok. This is an actively connected client.
           //  Let's grab a semaphore to prevent frame changes while we
           //  are serving this frame
+#if defined (BENCHMARK)
+          streamStart = micros();
+#endif
+
           xSemaphoreTake( frameSync, portMAX_DELAY );
 
-          client->write(CTNTTYPE, cntLen);
+#if defined (BENCHMARK)
+          waitAvg.value(micros()-streamStart);
+          frameAvg.value(camSize);
+          streamStart = micros();
+#endif
+
           sprintf(buf, "%d\r\n\r\n", camSize);
+          client->flush();
+          client->write(CTNTTYPE, cntLen);
           client->write(buf, strlen(buf));
           client->write((char*) camBuf, (size_t)camSize);
           client->write(BOUNDARY, bdrLen);
 
+#if defined (BENCHMARK)
+          streamAvg.value(micros()-streamStart);
+          fpsAvg.value(1000.0 / (float) (millis()-lastFrame) );
+          lastFrame = millis();
+#endif
+
+          //  The frame has been served. Release the semaphore and let other tasks run.
+          //  If there is a frame switch ready, it will happen now in between frames
+          xSemaphoreGive( frameSync );
+
           // Since this client is still connected, push it to the end
           // of the queue for further processing
           xQueueSend(streamingClients, (void *) &client, 0);
 
-          //  The frame has been served. Release the semaphore and let other tasks run.
-          //  If there is a frame switch ready, it will happen now in between frames
-          xSemaphoreGive( frameSync );
         }
       }
     }
@@ -180,7 +243,15 @@ void streamCB(void * pvParameters) {
       vTaskSuspend(NULL);
     }
     //  Let other tasks run after serving every client
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
+
+#if defined (BENCHMARK)
+    if ( millis() - lastPrint > BENCHMARK_PRINT_INT ) {
+      lastPrint = millis();
+      Log.verbose("streamCB: wait avg=%d, stream avg=%d us, frame avg size=%d bytes, fps=%S\n", waitAvg.currentValue(), streamAvg.currentValue(), frameAvg.currentValue(), String(fpsAvg.currentValue()));
+    }
+#endif
+
   }
 }
 

PlatformIO/esp32-cam-rtos-pio/src/streaming_multiclient_task.cpp

@@ -5,6 +5,12 @@
 volatile size_t   camSize;    // size of the current frame, byte
 volatile char*    camBuf;      // pointer to the current frame
 
+#if defined(BENCHMARK)
+#include <AverageFilter.h>
+#define BENCHMARK_PRINT_INT 1000
+averageFilter<uint32_t> captureAvg(10);
+uint32_t lastPrintCam = millis();
+#endif
 
 void camCB(void* pvParameters) {
 
@@ -23,34 +29,46 @@ void camCB(void* pvParameters) {
   xLastWakeTime = xTaskGetTickCount();
 
   for (;;) {
-
+    size_t s = 0;
     //  Grab a frame from the camera and query its size
     camera_fb_t* fb = NULL;
 
+#if defined(BENCHMARK)
+    uint32_t benchmarkStart = micros();
+#endif
+
+    s = 0;
     fb = esp_camera_fb_get();
-    size_t s = fb->len;
+    if ( fb ) {
+      s = fb->len;
 
       //  If frame size is more that we have previously allocated - request  125% of the current frame space
       if (s > fSize[ifb]) {
         fSize[ifb] = s + s/4;
-      fbs[ifb] = allocateMemory(fbs[ifb], fSize[ifb]);
+        fbs[ifb] = allocateMemory(fbs[ifb], fSize[ifb], FAIL_IF_OOM, ANY_MEMORY);
       }
 
       //  Copy current frame into local buffer
       char* b = (char *)fb->buf;
       memcpy(fbs[ifb], b, s);
       esp_camera_fb_return(fb);
+    }
+    else {
+      Log.error("camCB: error capturing image for frame %d\n", frameNumber);
+      vTaskDelay(1000);
+    }
 
-    //  Let other tasks run and wait until the end of the current frame rate interval (if any time left)
-    // taskYIELD();
-    vTaskDelayUntil(&xLastWakeTime, xFrequency);
+#if defined(BENCHMARK)
+    captureAvg.value(micros()-benchmarkStart);
+#endif
 
     //  Only switch frames around if no frame is currently being streamed to a client
     //  Wait on a semaphore until client operation completes
     //    xSemaphoreTake( frameSync, portMAX_DELAY );
 
     //  Do not allow frame copying while switching the current frame
-    xSemaphoreTake( frameSync, xFrequency );
+    // if ( xSemaphoreTake( frameSync, xFrequency ) ) {
+    if ( xSemaphoreTake( frameSync, portMAX_DELAY ) ) {
       camBuf = fbs[ifb];
       camSize = s;
       ifb++;
@@ -58,9 +76,10 @@ void camCB(void* pvParameters) {
       frameNumber++;
       //  Let anyone waiting for a frame know that the frame is ready
       xSemaphoreGive( frameSync );
+    }
 
-    //  Immediately let other (streaming) tasks run
-    taskYIELD();
+    //  Let other (streaming) tasks run
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
 
     //  If streaming task has suspended itself (no active clients to stream to)
     //  there is no need to grab frames from the camera. We can save some juice
@@ -72,6 +91,15 @@ void camCB(void* pvParameters) {
       Log.verbose("mjpegCB: tCam stack wtrmark  : %d\n", uxTaskGetStackHighWaterMark(tCam));
       vTaskSuspend(NULL);  // passing NULL means "suspend yourself"
     }
+
+#if defined(BENCHMARK)
+    if ( millis() - lastPrintCam > BENCHMARK_PRINT_INT ) {
+      lastPrintCam = millis();
+      Log.verbose("mjpegCB: average frame capture time: %d microseconds\n", captureAvg.currentValue() );
+    }
+#endif
+
+
   }
 }
 
@@ -83,8 +111,18 @@ void handleJPGSstream(void)
   Log.verbose("handleJPGSstream start: free heap  : %d\n", ESP.getFreeHeap());
 
   streamInfo_t* info = new streamInfo_t;
+  if ( info == NULL ) {
+    Log.error("handleJPGSstream: cannot allocate stream info - OOM\n");
+    return;
+  }
 
   WiFiClient* client = new WiFiClient();
+  if ( client == NULL ) {
+    Log.error("handleJPGSstream: cannot allocate WiFi client for streaming - OOM\n");
+    free(info);
+    return;
+  }
+
   *client = server.client();
 
   info->frame = frameNumber - 1;
@@ -138,37 +176,91 @@ void streamCB(void * pvParameters) {
   info->client->write(HEADER, hdrLen);
   info->client->write(BOUNDARY, bdrLen);
 
+#if defined(BENCHMARK)
+  averageFilter<int32_t> streamAvg(10);
+  averageFilter<int32_t> waitAvg(10);
+  averageFilter<uint32_t> frameAvg(10);
+  averageFilter<float> fpsAvg(10);
+  uint32_t streamStart = 0;
+  streamAvg.initialize();
+  waitAvg.initialize();
+  frameAvg.initialize();
+  fpsAvg.initialize();
+  uint32_t lastPrint = millis();
+  uint32_t lastFrame = millis();
+#endif
+
   for (;;) {
-    //  Only bother to send anything if there is someone watching
+    //  Only send anything if there is someone watching
     if ( info->client->connected() ) {
 
-      if ( info->frame != frameNumber) {
+      if ( info->frame != frameNumber) { // do not send same frame twice
+
+#if defined (BENCHMARK)
+        streamStart = micros();
+#endif        
+
         xSemaphoreTake( frameSync, portMAX_DELAY );
+        size_t currentSize = camSize;
+
+#if defined (BENCHMARK)
+        waitAvg.value(micros()-streamStart);
+        frameAvg.value(currentSize);
+        streamStart = micros();
+#endif
+
+
+//  ======================== OPTION1 ==================================
+//  server a copy of the buffer - overhead: have to allocate and copy a buffer for all frames
+
+// /*
         if ( info->buffer == NULL ) {
-          info->buffer = allocateMemory (info->buffer, camSize);
+          info->buffer = allocateMemory (info->buffer, camSize, FAIL_IF_OOM, ANY_MEMORY);
           info->len = camSize;
         }
         else {
           if ( camSize > info->len ) {
-            info->buffer = allocateMemory (info->buffer, camSize);
+            info->buffer = allocateMemory (info->buffer, camSize, FAIL_IF_OOM, ANY_MEMORY);
             info->len = camSize;
           }
         }
-        memcpy(info->buffer, (const void*) camBuf, info->len);
+        memcpy(info->buffer, (const void*) camBuf, camSize);
+        
         xSemaphoreGive( frameSync );
 
-        info->frame = frameNumber;
+        sprintf(buf, "%d\r\n\r\n", currentSize);
+        info->client->flush();
         info->client->write(CTNTTYPE, cntLen);
-        sprintf(buf, "%d\r\n\r\n", info->len);
         info->client->write(buf, strlen(buf));
-        info->client->write((char*) info->buffer, (size_t)info->len);
+        info->client->write((char*) info->buffer, currentSize);
         info->client->write(BOUNDARY, bdrLen);
+// */
+
+//  ======================== OPTION2 ==================================
+//  just server the comman buffer protected by mutex
+/*
+        sprintf(buf, "%d\r\n\r\n", camSize);
+        info->client->write(CTNTTYPE, cntLen);
+        info->client->write(buf, strlen(buf));
+        info->client->write((char*) camBuf, (size_t)camSize);
+
+        xSemaphoreGive( frameSync );
+
+        info->client->write(BOUNDARY, bdrLen);
+*/
+
+//  ====================================================================
+        info->frame = frameNumber;
+#if defined (BENCHMARK)
+          streamAvg.value(micros()-streamStart);
+          fpsAvg.value(1000.0 / (float) (millis()-lastFrame) );
+          lastFrame = millis();
+#endif        
       }
     }
     else {
       //  client disconnected - clean up.
       noActiveClients--;
-      Log.trace("streamCB: Client disconnected\n");
       Log.verbose("streamCB: Stream Task stack wtrmark  : %d\n", uxTaskGetStackHighWaterMark(info->task));
       info->client->stop();
       if ( info->buffer ) {
@@ -178,10 +270,19 @@ void streamCB(void * pvParameters) {
       delete info->client;
       delete info;
       info = NULL;
+      Log.trace("streamCB: Client disconnected\n");
+      vTaskDelay(100);
       vTaskDelete(NULL);
     }
     //  Let other tasks run after serving every client
-    if ( !xTaskDelayUntil(&xLastWakeTime, xFrequency) ) taskYIELD();
+    if ( xTaskDelayUntil(&xLastWakeTime, xFrequency) != pdTRUE ) taskYIELD();
+
+#if defined (BENCHMARK)
+    if ( millis() - lastPrint > BENCHMARK_PRINT_INT ) {
+      lastPrint = millis();
+      Log.verbose("streamCB: wait avg=%d, stream avg=%d us, frame avg size=%d bytes, fps=%S\n", waitAvg.currentValue(), streamAvg.currentValue(), frameAvg.currentValue(), String(fpsAvg.currentValue()));
+    }
+#endif
   }
 }