ESP32 Servo (iP send) + cam (udp)

[LBLBLBLB]

Hi esp32 helpers,

I am having some issues sending a UDP video at the same time as receiving servo commands by wifi

basically the issue is creating a header and fragmenting. I have looked at examples with RTSP but they are very different

Does anyone have any advice about creating a header and fragmenting for the images


#include <ESP32Servo.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <esp_camera.h>
#include <img_converters.h>


// Config
//// WIFI ID
#define WIFI_SSID "XXXX"
#define WIFI_PASSWORD "XXXX"
#define WIFI_SERVER_PORT 80
#define WIFI_UDP_PORT 4212
#define WIFI_BROADCAST_MASK 224,0,0,251

//// Servo limit
#define SERVO_MIN_ANGLE 40
#define SERVO_MAX_ANGLE 160

//// Camera
#define FLIP_V false // Vertical flip - inverse l'image verticalement
#define MIRROR_H false // Horizontal mirror - miroir horizontal
#define IMAGE_COMPRESSION 10 //0-63 lower number means higher quality

//// Intervals
#define WIFI_READ_CLIENT 50
#define WIFI_BROADCAST 42 // 42 approx 24 fps




// PINOUT
#define PIN_LED 33
#define PIN_FLASH 4
#define PIN_SERVO1 15
#define PIN_SERVO2 13
//// Camera
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27

#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22




// Declaration
typedef void (*Task)();

class RecurentTask {
public:
RecurentTask(Task task_ptr, int inter);
void update();
void setInterval(unsigned long);

private:
unsigned long previousMillis = 0;
unsigned long interval = 0;
Task task = NULL;
};

void blinkLED();
void WIFIConnect();
void WIFIReadClient();
void WIFIBroadCastVideo();



// Global vars
//// LED
bool LEDState;

//// WIFI
const char* ssid = WIFI_SSID;
const char* password = WIFI_PASSWORD;
WiFiServer wifiServer(WIFI_SERVER_PORT);
WiFiClient client;
WiFiUDP udp;
unsigned int destUdpPort = WIFI_UDP_PORT;
IPAddress broadcastMask(WIFI_BROADCAST_MASK);

//// Servo
Servo camservo1;
Servo camservo2;

//// RecurentTask
RecurentTask RT_WIFIReadClient(&WIFIReadClient, WIFI_READ_CLIENT);
RecurentTask RT_WIFIBroadCastVideo(&WIFIBroadCastVideo, WIFI_BROADCAST);

//// Camera
int camInit = -1;




// Setup
void setup() {
// set up Serial com
Serial.begin(115200);
Serial.println("Let's start !");

// Set pin mode LED
gpio_set_direction((gpio_num_t) PIN_LED, GPIO_MODE_OUTPUT);

// Set pin mode FLASH
gpio_set_direction((gpio_num_t) PIN_FLASH, GPIO_MODE_OUTPUT);

// Servo
camservo1.setPeriodHertz(50); // standard 50 hz servo
// camservo2.setPeriodHertz(50); // standard 50 hz servo

camservo1.attach(PIN_SERVO1, 500, 2000);
camservo2.attach(PIN_SERVO2,1000,2000);

}


// Loop
void loop() {
// state = WIFI not connected
if(WiFi.status() != WL_CONNECTED) {
WIFIConnect();
camInit = -1; // Force to init cam
}

if(camInit != 0) {
camInit = initCam();
}

if(WiFi.status() == WL_CONNECTED) {
// state = Check client
RT_WIFIReadClient.update();

if(camInit == 0) {
// state = broadcast video
RT_WIFIBroadCastVideo.update();
}
}
}




// Implementation
RecurentTask::RecurentTask(Task task_ptr, int inter) {
interval = inter;
task = task_ptr;
}

void RecurentTask::update() {
// check to see if it's time to do the task
unsigned long currentMillis = millis();

if( (currentMillis - previousMillis) >= interval ) {
(*task)();
previousMillis = currentMillis;
}
}

void RecurentTask::setInterval(unsigned long inter) {
interval = inter;
}

void blinkLED() {
LEDState = !LEDState;
gpio_set_level((gpio_num_t) PIN_LED, (gpio_num_t) LEDState);
}

int initCam() {
// setup camera
camera_config_t config;
config.ledc_channel = LEDC_CHANNEL_7;
config.ledc_timer = LEDC_TIMER_3;
config.pin_d0 = Y2_GPIO_NUM;
config.pin_d1 = Y3_GPIO_NUM;
config.pin_d2 = Y4_GPIO_NUM;
config.pin_d3 = Y5_GPIO_NUM;
config.pin_d4 = Y6_GPIO_NUM;
config.pin_d5 = Y7_GPIO_NUM;
config.pin_d6 = Y8_GPIO_NUM;
config.pin_d7 = Y9_GPIO_NUM;
config.pin_xclk = XCLK_GPIO_NUM;
config.pin_pclk = PCLK_GPIO_NUM;
config.pin_vsync = VSYNC_GPIO_NUM;
config.pin_href = HREF_GPIO_NUM;
config.pin_sscb_sda = SIOD_GPIO_NUM;
config.pin_sscb_scl = SIOC_GPIO_NUM;
config.pin_pwdn = PWDN_GPIO_NUM;
config.pin_reset = RESET_GPIO_NUM;
config.xclk_freq_hz = 20000000; //XCLK 20MHz or 10MHz
config.pixel_format = PIXFORMAT_JPEG; //YUV422,GRAYSCALE,RGB565,JPEG
config.frame_size = FRAMESIZE_SVGA; //UXGA SVGA VGA QVGA Do not use sizes above QVGA when not JPEG
config.jpeg_quality = IMAGE_COMPRESSION;
config.fb_count = 2; //if more than one, i2s runs in continuous mode. Use only with JPEG

// Camera init - Initialise la caméra
esp_err_t err = esp_camera_init(&config);
if (err != ESP_OK) {
Serial.println("Camera init failed with error:");
Serial.println(err);
return -1;
}

Serial.println("Camera correctly initialized");
sensor_t * s = esp_camera_sensor_get();
s->set_vflip(s, FLIP_V);
s->set_hmirror(s, MIRROR_H);

return 0;
}

void WIFIConnect() {

// close wifi & wifiserver if running TODO vérifier si ça marche bien
wifiServer.close();
udp.stop();
WiFi.disconnect();

// setup Wifi
WiFi.begin(ssid, password);

Serial.println("Connecting to WiFi..");
while (WiFi.status() != WL_CONNECTED) { }

Serial.println("Connected to the WiFi network");
Serial.println(WiFi.localIP());

udp.begin(destUdpPort);
delay(1000);

wifiServer.begin();
client = wifiServer.available();

}

void WIFIReadClient() {
// client available
camservo1.write(90);
camservo2.write(90);
Serial.println("reset");
delay(15);

if(!client) {
client = wifiServer.available();
} else {
if(client.available()) {
char c = client.read(); // read byte, position as bin data not ascii !!!!!!
int a=(int) c;
camservo1.write(a);
Serial.println("tiny");
camservo2.write(a);
Serial.println("big"+a);
Serial.println(String("angle int ") +String(a));
}
}
}

void WIFIBroadCastVideo() {
size_t _jpg_buf_len;
uint8_t * _jpg_buf;

// get camera img
camera_fb_t* fb = esp_camera_fb_get();

if (!fb) {
Serial.println("Camera capture failed");
} else {

if (fb->format != PIXFORMAT_JPEG) {

bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
if (!jpeg_converted) {
Serial.println("JPEG compression failed");
esp_camera_fb_return(fb);
free(_jpg_buf);
return;
}
} else {
_jpg_buf_len = fb->len;
_jpg_buf = fb->buf;
}

udp.beginPacket(IPAddress(224,0,0,251), WIFI_UDP_PORT);
int rtn = udp.write(_jpg_buf, _jpg_buf_len);
udp.endPacket();

esp_camera_fb_return(fb);

if (fb->format != PIXFORMAT_JPEG) {
free(_jpg_buf);
}

blinkLED();
}
}

[SParishwad]

Hi there @LBLBLBLB

No luck with this huh? I am trying to do the same.