espcamnrf/main/main.c

#include "driver/gpio.h"
#include "hal/gpio_types.h"
#include "sensor.h"
#include <stdint.h>
#include <stdio.h>

#define TAG "CAM"
#include "esp_camera.h"
#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 Y2_GPIO_NUM 5
#define Y3_GPIO_NUM 18
#define Y4_GPIO_NUM 19
#define Y5_GPIO_NUM 21
#define Y6_GPIO_NUM 36
#define Y7_GPIO_NUM 39
#define Y8_GPIO_NUM 34
#define Y9_GPIO_NUM 35

#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22

#define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE

#include "esp_log.h"
#include "esp_timer.h"
#include "nvs_flash.h"
#include "string.h"

#include "../components/mirf/mirf.h"
#include <driver/uart.h>

#define FEC_INTERVAL 3

#define BUF_SIZE 20000

typedef struct {
  int x, y, width, height;
} Rectangle;

#define MAX_CHUNK_DATA 28

typedef struct __attribute__((packed)) {
  uint8_t header[4];
  uint8_t data[28];
} RadioStruct;

typedef enum {
    CMD_SET_CHANNEL = 1,
    CMD_SET_RESOLUTION = 2,
    CMD_RESTART_FRAME = 3
} RadioCommandType;

uint8_t abort_sending = 0;

typedef struct __attribute__((packed)) {
    uint8_t cmd;       // CMD_SET_CHANNEL or CMD_SET_RESOLUTION
    uint16_t param1;   // new channel or width
    uint16_t param2;   // height (only used for resolution)
    uint16_t param3;
} RadioCommand;

framesize_t width_height_to_framesize(uint16_t w, uint16_t h) {
    if (w <= 96  && h <= 96)     return FRAMESIZE_96X96;
    if (w <= 128 && h <= 128)    return FRAMESIZE_128X128;
    if (w <= 160 && h <= 120)    return FRAMESIZE_QQVGA;
    if (w <= 176 && h <= 144)    return FRAMESIZE_QCIF;
    if (w <= 240 && h <= 176)    return FRAMESIZE_HQVGA;
    if (w <= 240 && h <= 240)    return FRAMESIZE_240X240;
    if (w <= 320 && h <= 240)    return FRAMESIZE_QVGA;
    if (w <= 320 && h <= 320)    return FRAMESIZE_320X320;
    if (w <= 400 && h <= 296)    return FRAMESIZE_CIF;
    if (w <= 480 && h <= 320)    return FRAMESIZE_HVGA;
    if (w <= 640 && h <= 480)    return FRAMESIZE_VGA;
    if (w <= 800 && h <= 600)    return FRAMESIZE_SVGA;
    if (w <= 1024 && h <= 768)   return FRAMESIZE_XGA;
    if (w <= 1280 && h <= 720)   return FRAMESIZE_HD;
    if (w <= 1280 && h <= 1024)  return FRAMESIZE_SXGA;
    if (w <= 1600 && h <= 1200)  return FRAMESIZE_UXGA;

    return FRAMESIZE_QVGA; // fallback if unknown
}
NRF24_t dev;

static void packHeader4_15bit(const uint8_t flags, const uint16_t chunkIndex,
                              const uint16_t totalChunks, uint8_t header[4]) {
  uint32_t bits = ((flags & 0x3) << 30) |         // 2 bits flags
                  ((chunkIndex & 0x7FFF) << 15) | // 15 bits chunkIndex
                  (totalChunks & 0x7FFF);         // 15 bits totalChunks
  header[0] = (bits >> 24) & 0xFF;
  header[1] = (bits >> 16) & 0xFF;
  header[2] = (bits >> 8) & 0xFF;
  header[3] = bits & 0xFF;
}

static void unpackHeader4_15bit(const uint8_t header[4], uint8_t *flags,
                                uint16_t *chunkIndex, uint16_t *totalChunks) {
  uint32_t bits =
      (header[0] << 24) | (header[1] << 16) | (header[2] << 8) | header[3];
  *flags = (bits >> 30) & 0x3;
  *chunkIndex = (bits >> 15) & 0x7FFF;
  *totalChunks = bits & 0x7FFF;
}


//
// Code start
//
int vsync_count = 0;
int sync_fail_count = 0;
int reset_fail_count = 0;
int64_t vsyncDetectedUsec;

static camera_config_t camera_config = {
    .pin_pwdn = PWDN_GPIO_NUM,
    .pin_reset = RESET_GPIO_NUM,
    .pin_xclk = XCLK_GPIO_NUM,

    .pin_sccb_sda = SIOD_GPIO_NUM,
    .pin_sccb_scl = SIOC_GPIO_NUM,

    .pin_d7 = Y9_GPIO_NUM,
    .pin_d6 = Y8_GPIO_NUM,
    .pin_d5 = Y7_GPIO_NUM,
    .pin_d4 = Y6_GPIO_NUM,
    .pin_d3 = Y5_GPIO_NUM,
    .pin_d2 = Y4_GPIO_NUM,
    .pin_d1 = Y3_GPIO_NUM,
    .pin_d0 = Y2_GPIO_NUM,
    .pin_vsync = VSYNC_GPIO_NUM,
    .pin_href = HREF_GPIO_NUM,
    .pin_pclk = PCLK_GPIO_NUM,

    .xclk_freq_hz = 10000000, // EXPERIMENTAL: Set to 16MHz on ESP32-S2 or
                              // ESP32-S3 to enable EDMA mode
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_JPEG, // YUV422,GRAYSCALE,RGB565,JPEG
    .frame_size =
        FRAMESIZE_UXGA, // QQVGA-UXGA, For ESP32, do not use sizes above QVGA
                        // when not JPEG. The performance of the ESP32-S series
                        // has improved a lot, but JPEG mode always gives better
                        // frame rates.+
    //.frame_size = FRAMESIZE_128X128, // QQVGA-UXGA, For ESP32, do not use
    // sizes above QVGA when not JPEG. The performance of the ESP32-S series has
    // improved a lot, but JPEG mode always gives better frame rates.

    .jpeg_quality = 63, // 0-63, for OV series camera sensors, lower number
                        // means higher quality
    .fb_count = 2, // When jpeg mode is used, if fb_count more than one, the
                   // driver will work in continuous mode.
    //.fb_location = CAMERA_FB_IN_PSRAM,
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_LATEST // CAMERA_GRAB_LATEST. Sets when buffers
                                    // should be filled
};

void handle_radio_commands() {
    uint8_t buf[32];
    if (Nrf24_dataReady(&dev)) {
        Nrf24_getData(&dev, buf);
        RadioCommand *cmd = (RadioCommand*)buf;

        switch (cmd->cmd) {
          case CMD_SET_CHANNEL:
              ESP_LOGI(TAG, "Changing RF channel to %d", cmd->param1);
              Nrf24_config(&dev, cmd->param1, 32); // reconfigure channel
              break;
          case CMD_SET_RESOLUTION:
              ESP_LOGI(TAG, "Changing resolution to %dx%d", cmd->param1, cmd->param2);
              camera_config.frame_size = width_height_to_framesize(cmd->param1, cmd->param2);
              uint8_t quality = cmd->param3;
              if (quality > 63) {
                quality = 63;
              }
              camera_config.jpeg_quality = quality;
              esp_camera_deinit();
              esp_camera_init(&camera_config);
              break;
          case CMD_RESTART_FRAME:
            abort_sending = 1;
            break;
          default:
              ESP_LOGW(TAG, "Unknown command %d", cmd->cmd);
        }
    }
}

// frames = array of RadioStruct data frames (not FEC frames)
// fecFrame = RadioStruct to write FEC frame into
// numFrames = number of frames to XOR
void createFECFrame(RadioStruct *frames, uint16_t numFrames,
                    RadioStruct *fecFrame) {

  uint8_t flags;
  uint16_t chunkIndex;
  uint16_t totalChunks;
  unpackHeader4_15bit(frames[0].header, &flags, &chunkIndex, &totalChunks);

  flags = 0x3; // mark as FEC
  // uint16_t totalChunks = numFrames;           // optional
  packHeader4_15bit(flags, chunkIndex, totalChunks, fecFrame->header);
  memset(fecFrame->data, 0, MAX_CHUNK_DATA);
  for (uint16_t i = 0; i < numFrames; i++) {
    for (uint8_t j = 0; j < MAX_CHUNK_DATA; j++) {
      fecFrame->data[j] ^= frames[i].data[j];
    }
  }
}

// dataPtr = pointer to JPEG data
// chunkIndex = index of this chunk
// chunkCount = total chunks
// flags = 0, START_OF_FRAME, END_OF_FRAME, or FEC
// outFrame = pointer to RadioStruct to fill
void createFrame(const uint8_t *dataPtr, size_t jpegLen, uint16_t chunkIndex,
                 uint16_t totalChunks, uint8_t flags, RadioStruct *outFrame) {
  packHeader4_15bit(flags, chunkIndex, totalChunks, outFrame->header);
  size_t remaining = jpegLen - chunkIndex * MAX_CHUNK_DATA;
  size_t copyLen = remaining > MAX_CHUNK_DATA ? MAX_CHUNK_DATA : remaining;

  memcpy(outFrame->data, dataPtr + chunkIndex * MAX_CHUNK_DATA, copyLen);
  if (copyLen < MAX_CHUNK_DATA) {
    memset(outFrame->data + copyLen, 0, MAX_CHUNK_DATA - copyLen); // pad last
  }

  /*
  ESP_LOGI(TAG, "Flags: 0x%02x, chunk index: %04d, chunk count: %04d", flags,
           chunkIndex, totalChunks);
  uint8_t *frmPTR = (uint8_t *)outFrame;
  ESP_LOGI(TAG, "Bytes: 0x%02x 0x%02x 0x%02x", frmPTR[0], frmPTR[1], frmPTR[2]);
  */
}

void sendPacket(const RadioStruct * packet) {
  uint8_t packetData[32];
  memcpy(packetData, packet->header, 4);
  memcpy(&(packetData[4]), packet->data, 28);
  Nrf24_send(&dev, (uint8_t *)&packetData);
    if (Nrf24_isSend(&dev, 1000)) {
      // ESP_LOGI(pcTaskGetName(NULL), "Send success.");
    } else {
      // ESP_LOGW(pcTaskGetName(NULL), "Send fail.");
    }
  handle_radio_commands();
}

void send_jpeg_nrf(const uint8_t *jpegData, size_t jpegLen) {
  // calculate total chunks
  uint16_t totalChunks = (jpegLen + MAX_CHUNK_DATA - 1) / MAX_CHUNK_DATA;

  RadioStruct frameBuffer[FEC_INTERVAL]; // buffer to store frames for FEC
  uint16_t fecCount = 0;

  for (uint16_t chunkIndex = 0; chunkIndex < totalChunks; chunkIndex++) {
    if (abort_sending == 1) {
      abort_sending = 0;
      break;
    }
    uint8_t flags = 0;
    if (chunkIndex == 0) {
      flags |= 0x1;
      ESP_LOGI(TAG, "Flag start");
    }
    if (chunkIndex == totalChunks - 1) {
      flags |= 0x2;
      ESP_LOGI(TAG, "Flag end");
    }

    RadioStruct packet;
    createFrame(jpegData, jpegLen, chunkIndex, totalChunks, flags, &packet);
    frameBuffer[fecCount++] = packet;

    sendPacket(&packet);
    if (fecCount == FEC_INTERVAL) {
      RadioStruct fecPacket;
      createFECFrame(frameBuffer, FEC_INTERVAL, &fecPacket);
      sendPacket(&fecPacket);
      fecCount = 0;
    }
  }

  // send FEC for leftover frames
  if (fecCount > 0) {
    RadioStruct fecPacket;
    createFECFrame(frameBuffer, fecCount, &fecPacket);
    sendPacket(&fecPacket);
  }
}


esp_err_t camera_init() {
  // initialize the camera
  esp_err_t err = esp_camera_init(&camera_config);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "Camera Init Failed");
    return err;
  }

  return ESP_OK;
}

typedef struct __attribute__((packed)) {
  uint32_t len;
  uint16_t width;
  uint16_t height;
  pixformat_t format;
  struct timeval timestamp;
} PreData;

uint8_t *create_frame(const uint8_t *data, size_t *outSize, PreData pre) {

  
  
  const size_t totalSize = pre.len + 23;
  ESP_LOGI(TAG, "DataWithFrame: %d\n", totalSize);
  uint8_t *dataOut = malloc(totalSize);
  if (!dataOut) {
    ESP_LOGE(TAG, "Failed to allocate memory for frame");
    return NULL;
  }

  size_t bytePos = 0;
  
  int64_t time_us =
  (int64_t)pre.timestamp.tv_sec * 1000000L + (int64_t)pre.timestamp.tv_usec;
  
  dataOut[bytePos++] = 'B';
  dataOut[bytePos++] = 'R';
  dataOut[bytePos++] = 'N';
  dataOut[bytePos++] = 'F';
  dataOut[bytePos++] = 'R';
  dataOut[bytePos++] = 'M';
  dataOut[bytePos++] = (pre.len >> 24) & 0xFF;
  dataOut[bytePos++] = (pre.len >> 16) & 0xFF;
  dataOut[bytePos++] = (pre.len >> 8) & 0xFF;
  dataOut[bytePos++] = (pre.len) & 0xFF;
  dataOut[bytePos++] = (pre.width >> 8) & 0xFF;
  dataOut[bytePos++] = (pre.width) & 0xFF;
  dataOut[bytePos++] = (pre.height >> 8) & 0xFF;
  dataOut[bytePos++] = (pre.height) & 0xFF;
  dataOut[bytePos++] = (pre.format) & 0xFF;
  dataOut[bytePos++] = (time_us >> 56) & 0xFF;
  dataOut[bytePos++] = (time_us >> 48) & 0xFF;
  dataOut[bytePos++] = (time_us >> 40) & 0xFF;
  dataOut[bytePos++] = (time_us >> 32) & 0xFF;
  dataOut[bytePos++] = (time_us >> 24) & 0xFF;
  dataOut[bytePos++] = (time_us >> 16) & 0xFF;
  dataOut[bytePos++] = (time_us >> 8) & 0xFF;
  dataOut[bytePos++] = (time_us) & 0xFF;
  
  ESP_LOGI(TAG,
    "Len: %ld, width:%d, height:%d, format:%d, timestamp: %lld",
    pre.len, pre.width, pre.height, pre.format, time_us);
    // Copy image data after PreData
  memcpy(dataOut + bytePos, data, pre.len);

  *outSize = totalSize;
  return dataOut;
}

esp_err_t camera_capture() {
  gpio_set_level(GPIO_NUM_4, 1);
  ESP_LOGI(TAG, "Pregrab");
  camera_fb_t *fb = esp_camera_fb_get();
  ESP_LOGI(TAG, "Postgrab");

  if (!fb) {
    ESP_LOGE(TAG, "Camera Capture Failed");
    return ESP_FAIL;
  }

  PreData pre;
  pre.len = fb->len;
  pre.width = fb->width;
  pre.height = fb->height;
  pre.format = fb->format;
  pre.timestamp = fb->timestamp;

  size_t totalSize;
  uint8_t *dataOut = create_frame(fb->buf, &totalSize, pre);
  if (!dataOut) {
    esp_camera_fb_return(fb);
    return ESP_ERR_NO_MEM;
  }

  esp_camera_fb_return(fb);
  gpio_set_level(GPIO_NUM_4, 0);
  send_jpeg_nrf(dataOut, totalSize);
  
  free(dataOut);
  
  /*
  uint8_t testData[1000];
  memcpy(testData, "Lorem ipsum dolor sit amet, consectetur adipiscing elit. Proin ut felis vitae magna laoreet interdum dictum auctor risus. Quisque quis enim sagittis, venenatis dui sed, fermentum tortor. Fusce euismod vulputate felis at commodo. Ut ac fermentum metus. Proin in nunc ac libero accumsan vehicula. Praesent posuere nulla sit amet odio blandit, in pharetra odio dapibus. Nulla id ultrices erat. Cras porta sed nunc vitae ultricies. Duis luctus ipsum quis mauris maximus, in venenatis dui venenatis. Integer nec tellus turpis. Nulla erat sapien, tempus at velit sit amet, maximus euismod ipsum. Vivamus ac orci mi. Sed elementum tortor ac nisi lobortis, in sodales est lacinia.\n\nVestibulum a eros ut metus imperdiet finibus. Vivamus vel lacus blandit, accumsan massa ut, elementum lorem. Maecenas ac neque nibh. Integer et nisi pellentesque, iaculis augue vitae, lobortis urna. Nullam venenatis viverra odio, sed molestie orci sagittis in. Curabitur iaculis sem finibus arcu viverra, quis imperdiet libe", 1000);

  PreData pre;
  pre.len = sizeof(testData);
  pre.width = fb->width;
  pre.height = fb->height;
  pre.format = fb->format;
  pre.timestamp = fb->timestamp;

  size_t totalSize;
  uint8_t *dataOut = create_frame(testData, &totalSize, pre);
  if (!dataOut) {
    esp_camera_fb_return(fb);
    return ESP_ERR_NO_MEM;
  }

  esp_camera_fb_return(fb);
  send_jpeg_nrf(dataOut, totalSize);
  
  free(dataOut);
  */
  return ESP_OK;
}

void init_nrf() {
  Nrf24_init(&dev);
  uint8_t payload = 32;
  uint8_t channel = CONFIG_RADIO_CHANNEL;
  Nrf24_config(&dev, channel, payload);

  // Set destination address using 5 characters
  //esp_err_t ret = Nrf24_setTADDR(&dev, (uint8_t *)"CAMRX");
  esp_err_t ret = Nrf24_setTADDR(&dev, (uint8_t *)"CAM");
  if (ret != ESP_OK) {
    ESP_LOGE(pcTaskGetName(NULL), "nrf24l01 not installed");
    while (1) {
      vTaskDelay(1);
    }
  }
  ret = Nrf24_setRADDR(&dev, (uint8_t *)"CAM");
  if (ret != ESP_OK) {
    ESP_LOGE(pcTaskGetName(NULL), "nrf24l01 not installed");
    while (1) {
      vTaskDelay(1);
    }
  }

  
  ESP_LOGW(pcTaskGetName(NULL), "Set RF Data Ratio to 250kBps");
  Nrf24_SetSpeedDataRates(&dev, 2);
  

  /*
  ESP_LOGW(pcTaskGetName(NULL), "Set RF Data Ratio to 2MBps");
  Nrf24_SetSpeedDataRates(&dev, 1);
  */
  
  
  ESP_LOGW(pcTaskGetName(NULL), "Set RF max retransmits to 4 tries");
  Nrf24_setRetransmitCount(&dev, 15);

  ESP_LOGW(pcTaskGetName(NULL), "CONFIG_RETRANSMIT_DELAY=2mS");
  Nrf24_setRetransmitDelay(&dev, 8);

  // Print settings
  Nrf24_printDetails(&dev);
}


void main_task(void *arg) {

  init_nrf();
  gpio_set_direction(4, GPIO_MODE_DEF_OUTPUT);
  ESP_ERROR_CHECK(camera_init());
  while (1) {

    int64_t start = esp_timer_get_time();
    camera_capture();
    handle_radio_commands();

    int64_t end = esp_timer_get_time();

    int64_t duration = end - start;

    printf("%lf FPS, %lf SPF\n", 1 / (duration / 1000000.0),
           duration / 1000000.0);
  }
}

void app_main(void) {

  const uart_port_t uart_num = UART_NUM_0;
  uart_config_t uart_config = {.baud_rate = 115200,
                               .data_bits = UART_DATA_8_BITS,
                               .parity = UART_PARITY_DISABLE,
                               .stop_bits = UART_STOP_BITS_1,
                               .flow_ctrl = UART_HW_FLOWCTRL_DISABLE};
  uart_param_config(uart_num, &uart_config);
  uart_driver_install(uart_num, BUF_SIZE * 2, 0, 0, NULL, 0);

  nvs_flash_init();

  xTaskCreate(main_task, "main_task", 8192, NULL, 5, NULL); // 8 KB stack
}