Skip to content

Commit

Permalink
Merge pull request #3 from hpsaturn/espnow_init_improv
Browse files Browse the repository at this point in the history 
Espnow payload improvement
  • Loading branch information
@hpsaturn
hpsaturn authored Jan 29, 2024
2 parents 1135ca1 + 857cafc commit 04c4a90
Show file tree
Hide file tree
Showing 13 changed files with 326 additions and 512 deletions.
130 changes: 20 additions & 110 deletions examples/core2-espnow-receiver/core2-espnow-receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,109 +4,21 @@
* This file is part ESP32S3 camera tests project:
* https://github.com/hpsaturn/esp32s3-cam
**************************************************/

#include <Arduino.h>
#include <Wire.h>
#include <M5Unified.h>
#include <WiFi.h>
#include <esp_now.h>
#include <pb_decode.h>
#include "frame.pb.h"
#include "ESPNowReceiver.h"
#include "Utils.h"

int32_t dw, dh;
ESPNowReceiver radio;

/// general buffer for receive msgs
uint8_t recv_buffer[256];
/// frame buffer
// frame buffer
uint8_t *fb;
bool is_new_frame = true;

Frame msg = Frame_init_zero;

uint32_t fbpos = 0;
// uint32_t cksum = 0;

bool decode_data(pb_istream_t *stream, const pb_field_t *field, void **arg) {
uint64_t value;
if (!pb_decode_varint(stream, &value))
return false;
// cksum = cksum + (uint8_t) value;
fb[fbpos++] = (uint8_t) value;
return true;
}

bool decodeMessage(uint16_t message_length) {
pb_istream_t stream = pb_istream_from_buffer(recv_buffer, message_length);
msg.data.funcs.decode = &decode_data;
// msg.data.arg = (void*) "array: \"%d\"\r\n";
bool status = pb_decode(&stream, Frame_fields, &msg);
if (!status) {
Serial.printf("Decoding msg failed: %s\r\n", PB_GET_ERROR(&stream));
return false;
}
return true;
}

uint16_t frame = 0;

void printFPS(const char *msg) {
static uint_least64_t timeStamp = 0;
frame++;
if (millis() - timeStamp > 1000) {
timeStamp = millis();
Serial.printf("%s %2d FPS\r\n",msg, frame);
frame = 0;
}
}

void printFB(uint32_t len){
for (int32_t i = 0; i < len; i++) {
Serial.printf("%i ",fb[i]);
}
}

void msgReceiveCb(const uint8_t *macAddr, const uint8_t *data, int dataLen) {
int msgLen = min(ESP_NOW_MAX_DATA_LEN, dataLen);
// BE CAREFUL WITH IT, IF JPG LEVEL CHANGES, INCREASE IT
if (is_new_frame) fb = (uint8_t*) malloc(4000* sizeof( uint8_t ) ) ;
memcpy(recv_buffer, data, msgLen);
if (decodeMessage(msgLen)) {
is_new_frame = false;
// Serial.println();
if (msg.lenght > 0 ) {
// printFB(msg.lenght);
// Serial.printf("\r\nfb size: %i msg lenght: %i cksum: %u\r\n",fbpos,msg.lenght,cksum);
M5.Display.drawJpg(fb, msg.lenght , 0, 0, dw, dh);
free(fb);
is_new_frame = true;
fbpos = 0;
// cksum = 0;
printFPS("ESPNow reception at:");
}
// Serial.printf("chunk len: %d\r\n",msg.lenght);
}
}

bool ESPNowInit() {
WiFi.mode(WIFI_STA);
// startup ESP Now
Serial.println("ESPNow Init");
Serial.println(WiFi.macAddress());
// shutdown wifi
WiFi.disconnect();
delay(100);
// display globals
int32_t dw, dh;

if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success");
esp_now_register_recv_cb(msgReceiveCb);
Serial.println("Registered receiver callback");
// esp_now_register_send_cb(telemetrySendCallback);
return true;
} else {
Serial.println("ESPNow Init Failed");
delay(100);
ESP.restart();
return false;
}
void onDataReady(uint32_t lenght) {
M5.Display.drawJpg(fb, lenght , 0, 0, dw, dh);
}

void setup() {
Expand All @@ -117,23 +29,21 @@ void setup() {
dw=M5.Display.width();
dh=M5.Display.height();

WiFi.mode(WIFI_STA);
// startup ESP Now
Serial.println("ESPNow Init");
Serial.println(WiFi.macAddress());
// shutdown wifi
WiFi.disconnect();
delay(100);

if (ESPNowInit()) {
M5.Display.drawString("ESPNow Init Success", dw / 2, dh / 2);
}

if(psramFound()){
size_t psram_size = esp_spiram_get_size() / 1048576;
Serial.printf("PSRAM size: %dMb\r\n", psram_size);
}
delay(1000);

// BE CAREFUL WITH IT, IF JPG LEVEL CHANGES, INCREASE IT
fb = (uint8_t*) ps_malloc(5000* sizeof( uint8_t ) ) ;

radio.setRecvBuffer(fb);
radio.setRecvCallback(onDataReady);

if (radio.init()) {
M5.Display.drawString("ESPNow Init Success", dw / 2, dh / 2);
}
delay(500);
}

void loop() {
Expand Down
1 change: 1 addition & 0 deletions examples/freenove-basic/freenove-basic.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <Arduino.h>
#include <WiFi.h>
#include <CamFreenove.h>

// #define CONVERT_TO_JPEG
Expand Down
160 changes: 15 additions & 145 deletions examples/freenove-espnow/freenove-espnow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,34 +6,30 @@
**************************************************/

#include <Arduino.h>
#include <WiFi.h>
#include <esp_now.h>
#include <pb_encode.h>
#include "CamFreenove.h"
#include "frame.pb.h"

#define CHUNKSIZE 87
#include "ESPNowSender.h"
#include "Utils.h"

CamFreenove Camera;
ESPNowSender radio;

/// general buffer for msg sender
uint8_t send_buffer[256];
void processFrame() {
if (Camera.get()) {
uint8_t *out_jpg = NULL;
size_t out_jpg_len = 0;
frame2jpg(Camera.fb, 12, &out_jpg, &out_jpg_len);
radio.sendData(out_jpg, out_jpg_len);
// printFPS("CAM:");
free(out_jpg);
Camera.free();
}
}

void setup() {
Serial.begin(115200);
Serial.setDebugOutput(true);
Serial.println();
delay(1000);
WiFi.mode(WIFI_STA);
Serial.println(WiFi.macAddress());
// shutdown wifi
WiFi.disconnect();
delay(100);

while (esp_now_init() != ESP_OK) {
Serial.print(".");
}
Serial.println("\r\nESPNow Init Success");

if (!Camera.begin()) {
Serial.println("Camera Init Fail");
Expand All @@ -44,136 +40,10 @@ void setup() {
size_t psram_size = esp_spiram_get_size() / 1048576;
Serial.printf("PSRAM size: %dMb\r\n", psram_size);
}
radio.init();
delay(500);
}

uint16_t frame = 0;

void printFPS(const char *msg) {
static uint_least64_t timeStamp = 0;
frame++;
if (millis() - timeStamp > 1000) {
timeStamp = millis();
Serial.printf("%s %2d FPS\r\n",msg, frame);
frame = 0;
}
}

size_t encodeMsg(Frame msg) {
pb_ostream_t stream = pb_ostream_from_buffer(send_buffer, sizeof(send_buffer));
bool status = pb_encode(&stream, Frame_fields, &msg);
size_t message_length = stream.bytes_written;
if (!status) {
printf("Encoding failed: %s\r\n", PB_GET_ERROR(&stream));
return 0;
}
return message_length;
}

bool sendMessage(uint32_t msglen, const uint8_t *mac) {
esp_now_peer_info_t peerInfo = {};
memcpy(&peerInfo.peer_addr, mac, 6);
if (!esp_now_is_peer_exist(mac)) {
esp_now_add_peer(&peerInfo);
}
esp_err_t result = esp_now_send(mac, send_buffer, msglen);

if (result == ESP_OK) {
// Serial.println("send msg success");
return true;
} else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
Serial.println("ESPNOW not Init.");
} else if (result == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument");
} else if (result == ESP_ERR_ESPNOW_INTERNAL) {
Serial.println("Internal Error");
} else if (result == ESP_ERR_ESPNOW_NO_MEM) {
Serial.println("ESP_ERR_ESPNOW_NO_MEM");
} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
Serial.println("Peer not found.");
} else {
Serial.println("Unknown error");
}
return false;
}

uint8_t targetAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

uint8_t *out_jpg = NULL;
size_t out_jpg_len = 0;
uint8_t chunk_size = CHUNKSIZE;
uint32_t chunk_pos = 0;
uint32_t framesum = 0;
uint32_t bytecount = 0;


bool encode_uint8_array(pb_ostream_t *stream, const pb_field_t *field, void *const *arg) {
for (int i = 0; i < chunk_size; i++) {
if (!pb_encode_tag_for_field(stream, field))
return false;
uint8_t val = (out_jpg+chunk_pos)[i];
// framesum = framesum + val;
// Serial.printf("%i ", val);
// bytecount++;
if (!pb_encode_varint(stream, val))
return false;
}
// Serial.printf("%i ", chunk_pos);
return true;
}

void dispatchFrame() {
// Serial.println("Encoded Frame:");
uint32_t chunk_left = out_jpg_len;
Frame msg = Frame_init_zero;
while (chunk_left > 0) {
if (chunk_left <= chunk_size) {
chunk_size = chunk_left;
msg.lenght = out_jpg_len;
}
else {
chunk_left = chunk_left - chunk_size;
msg.lenght = 0;
}
msg.data.funcs.encode = &encode_uint8_array;
sendMessage(encodeMsg(msg), targetAddress);
chunk_pos = out_jpg_len - chunk_left;
if (msg.lenght == out_jpg_len) {
chunk_left = 0;
chunk_size = CHUNKSIZE;
}
delay(4);
}
chunk_pos = 0;
// Serial.printf("\r\nFrame encoded lenght: %u cheksum: %u\r\n", bytecount, framesum);
// bytecount = 0;
// framesum = 0;
}

void printJPGFrame(){
uint32_t checksum = 0;
Serial.println("JPG Frame:");
for (int i = 0; i < out_jpg_len; i++) {
checksum = checksum + (uint8_t) out_jpg[i];
Serial.printf("%i ", out_jpg[i]);
}
Serial.printf("\r\nJPG lenght: %u cheksum: %u\r\n", out_jpg_len, checksum);
}

void processFrame() {
if (Camera.get()) {
frame2jpg(Camera.fb, 9, &out_jpg, &out_jpg_len);
// Display.pushImage(0, 0, dw, dh, (uint16_t *)CoreS3.Camera.fb->buf);
// printJPGFrame();
// Serial.println();
dispatchFrame();
// Serial.println(out_jpg_len);
// printFPS("transmitting at ");
free(out_jpg);
Camera.free();
}
}

void loop() {
processFrame();
}
4 changes: 2 additions & 2 deletions examples/m5cores3-basic/m5cores3-basic.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#include "M5CoreS3.h"
#include "esp_camera.h"
#include <M5CoreS3.h>
#include <esp_camera.h>

int32_t dw, dh;

Expand Down
Loading

0 comments on commit 04c4a90

Please sign in to comment.