252 lines
9.0 KiB
C++
252 lines
9.0 KiB
C++
// DFR1154 V1.1 person-position experiment using Espressif's pedestrian model.
|
|
// Camera pin assignments match DFRobot's DFR1154 CameraWebServer example.
|
|
|
|
#include <algorithm>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
|
|
#include "dl_image_jpeg.hpp"
|
|
#include "driver/gpio.h"
|
|
#include "esp_camera.h"
|
|
#include "esp_heap_caps.h"
|
|
#include "esp_log.h"
|
|
#include "esp_timer.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "pedestrian_detect.hpp"
|
|
#include "driver/uart.h"
|
|
#include "../../shared/GazeProtocol.h"
|
|
#include "../../shared/TrackingGate.h"
|
|
#include "ota_service.h"
|
|
|
|
namespace {
|
|
constexpr char kTag[] = "freddy_person";
|
|
constexpr float kDiagnosticScoreThreshold = 0.35f;
|
|
constexpr gpio_num_t kIrLedPin = GPIO_NUM_47;
|
|
constexpr uart_port_t kGazeUart = UART_NUM_1;
|
|
constexpr int kGazeTxPin = 43;
|
|
constexpr int kGazeBaud = 115200;
|
|
|
|
void init_gaze_uart() {
|
|
const uart_config_t config = {
|
|
.baud_rate = kGazeBaud,
|
|
.data_bits = UART_DATA_8_BITS,
|
|
.parity = UART_PARITY_DISABLE,
|
|
.stop_bits = UART_STOP_BITS_1,
|
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
|
.rx_flow_ctrl_thresh = 0,
|
|
.source_clk = UART_SCLK_DEFAULT,
|
|
.flags = {},
|
|
};
|
|
ESP_ERROR_CHECK(uart_driver_install(kGazeUart, 1024, 0, 0, nullptr, 0));
|
|
ESP_ERROR_CHECK(uart_param_config(kGazeUart, &config));
|
|
ESP_ERROR_CHECK(uart_set_pin(kGazeUart, kGazeTxPin, UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
|
ESP_LOGI(kTag, "GAZE_UART_READY tx_gpio=%d baud=%d", kGazeTxPin, kGazeBaud);
|
|
}
|
|
|
|
void send_gaze(uint16_t sequence, int x, int y, bool valid) {
|
|
gaze::Frame command = {sequence, static_cast<uint16_t>(x),
|
|
static_cast<uint16_t>(y), valid};
|
|
uint8_t bytes[gaze::kFrameSize];
|
|
gaze::encode(command, bytes);
|
|
if (uart_write_bytes(kGazeUart, bytes, sizeof(bytes)) != sizeof(bytes)) {
|
|
ESP_LOGW(kTag, "GAZE_UART_WRITE_ERROR seq=%u", sequence);
|
|
}
|
|
}
|
|
|
|
#ifndef FREDDY_GAZE_LINK_SELF_TEST
|
|
#define FREDDY_GAZE_LINK_SELF_TEST 0
|
|
#endif
|
|
|
|
#if FREDDY_GAZE_LINK_SELF_TEST
|
|
// Bench-only UART fault injection. Keep disabled in the normal camera build.
|
|
void run_gaze_link_self_test() {
|
|
uint8_t bytes[gaze::kFrameSize];
|
|
gaze::encode({0xF000, 500, 500, false}, bytes);
|
|
uart_write_bytes(kGazeUart, bytes, sizeof(bytes));
|
|
vTaskDelay(pdMS_TO_TICKS(150));
|
|
uart_write_bytes(kGazeUart, bytes, sizeof(bytes)); // Duplicate sequence.
|
|
vTaskDelay(pdMS_TO_TICKS(150));
|
|
gaze::encode({0xEFFF, 500, 500, false}, bytes);
|
|
uart_write_bytes(kGazeUart, bytes, sizeof(bytes)); // Old sequence.
|
|
vTaskDelay(pdMS_TO_TICKS(150));
|
|
gaze::encode({0xF001, 500, 500, false}, bytes);
|
|
bytes[2] = 2; // Wrong version.
|
|
uart_write_bytes(kGazeUart, bytes, sizeof(bytes));
|
|
vTaskDelay(pdMS_TO_TICKS(150));
|
|
gaze::encode({0xF001, 500, 500, false}, bytes);
|
|
bytes[10] ^= 1; // Damaged CRC.
|
|
uart_write_bytes(kGazeUart, bytes, sizeof(bytes));
|
|
vTaskDelay(pdMS_TO_TICKS(150));
|
|
send_gaze(0xF002, 500, 500, false); // Recovery.
|
|
ESP_LOGI(kTag, "GAZE_LINK_SELF_TEST_SENT");
|
|
}
|
|
#endif
|
|
|
|
#ifndef FREDDY_CAMERA_DIAGNOSTIC_JPEG
|
|
#define FREDDY_CAMERA_DIAGNOSTIC_JPEG 0
|
|
#endif
|
|
|
|
#if FREDDY_CAMERA_DIAGNOSTIC_JPEG
|
|
// Optional bench diagnostic: one JPEG roughly every five seconds.
|
|
void print_jpeg_for_diagnostics(const camera_fb_t *frame, unsigned frame_number) {
|
|
printf("FRAME_BEGIN %u %u\n", frame_number, static_cast<unsigned>(frame->len));
|
|
for (size_t offset = 0; offset < frame->len; offset += 48) {
|
|
printf("FRAME_HEX ");
|
|
for (size_t i = offset; i < frame->len && i < offset + 48; ++i) {
|
|
printf("%02x", frame->buf[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("FRAME_END %u\n", frame_number);
|
|
fflush(stdout);
|
|
}
|
|
#endif
|
|
|
|
camera_config_t camera_config() {
|
|
camera_config_t config = {};
|
|
config.ledc_channel = LEDC_CHANNEL_0;
|
|
config.ledc_timer = LEDC_TIMER_0;
|
|
config.pin_d0 = 16;
|
|
config.pin_d1 = 18;
|
|
config.pin_d2 = 21;
|
|
config.pin_d3 = 17;
|
|
config.pin_d4 = 14;
|
|
config.pin_d5 = 7;
|
|
config.pin_d6 = 6;
|
|
config.pin_d7 = 4;
|
|
config.pin_xclk = 5;
|
|
config.pin_pclk = 15;
|
|
config.pin_vsync = 1;
|
|
config.pin_href = 2;
|
|
config.pin_sccb_sda = 8;
|
|
config.pin_sccb_scl = 9;
|
|
config.pin_pwdn = -1;
|
|
config.pin_reset = -1;
|
|
config.xclk_freq_hz = 20000000;
|
|
config.pixel_format = PIXFORMAT_JPEG;
|
|
config.frame_size = FRAMESIZE_QVGA;
|
|
config.jpeg_quality = 12;
|
|
config.fb_count = 1;
|
|
config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
|
|
config.fb_location = CAMERA_FB_IN_PSRAM;
|
|
return config;
|
|
}
|
|
} // namespace
|
|
|
|
extern "C" void app_main(void) {
|
|
init_gaze_uart();
|
|
#if FREDDY_GAZE_LINK_SELF_TEST
|
|
run_gaze_link_self_test();
|
|
#endif
|
|
ESP_ERROR_CHECK(gpio_reset_pin(kIrLedPin));
|
|
ESP_ERROR_CHECK(gpio_set_direction(kIrLedPin, GPIO_MODE_OUTPUT));
|
|
ESP_ERROR_CHECK(gpio_set_level(kIrLedPin, 1));
|
|
ESP_LOGI(kTag, "IR_GPIO47_HIGH");
|
|
|
|
camera_config_t config = camera_config();
|
|
const esp_err_t camera_error = esp_camera_init(&config);
|
|
if (camera_error != ESP_OK) {
|
|
ESP_LOGE(kTag, "CAMERA_INIT_ERROR 0x%X", camera_error);
|
|
return;
|
|
}
|
|
sensor_t *sensor = esp_camera_sensor_get();
|
|
if (sensor) {
|
|
ESP_LOGI(kTag, "SENSOR_PID 0x%04X", sensor->id.PID);
|
|
if (sensor->id.PID == OV3660_PID) sensor->set_vflip(sensor, 1);
|
|
}
|
|
|
|
auto *detector = new PedestrianDetect();
|
|
detector->set_score_thr(kDiagnosticScoreThreshold);
|
|
ESP_LOGI(kTag, "PERSON_DETECTOR_READY threshold=%.2f", kDiagnosticScoreThreshold);
|
|
ota_service_start();
|
|
ota_service_confirm_healthy();
|
|
|
|
unsigned frame_number = 0;
|
|
uint16_t gaze_sequence = 0;
|
|
tracking::Gate target_gate;
|
|
for (;;) {
|
|
const int64_t capture_started = esp_timer_get_time();
|
|
camera_fb_t *frame = esp_camera_fb_get();
|
|
if (!frame) {
|
|
ESP_LOGW(kTag, "CAPTURE_ERROR");
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
continue;
|
|
}
|
|
++frame_number;
|
|
#if FREDDY_CAMERA_DIAGNOSTIC_JPEG
|
|
if (frame_number % 20 == 1) {
|
|
print_jpeg_for_diagnostics(frame, frame_number);
|
|
}
|
|
#endif
|
|
dl::image::jpeg_img_t jpeg = {
|
|
.data = frame->buf,
|
|
.data_len = frame->len,
|
|
};
|
|
auto image = dl::image::sw_decode_jpeg(jpeg, dl::image::DL_IMAGE_PIX_TYPE_RGB888);
|
|
esp_camera_fb_return(frame);
|
|
if (!image.data) {
|
|
ESP_LOGW(kTag, "DECODE_ERROR");
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
continue;
|
|
}
|
|
|
|
auto &results = detector->run(image);
|
|
const int64_t completed = esp_timer_get_time();
|
|
const dl::detect::result_t *best = nullptr;
|
|
int rejected_full_frame = 0;
|
|
int rejected_full_height = 0;
|
|
for (const auto &result : results) {
|
|
const int box_width = result.box[2] - result.box[0];
|
|
const int box_height = result.box[3] - result.box[1];
|
|
// The empty-scene diagnostic repeatedly labels almost the entire image
|
|
// as a person. Keep this shape from commanding either eye.
|
|
if (box_width >= image.width * 9 / 10 &&
|
|
box_height >= image.height * 9 / 10) {
|
|
++rejected_full_frame;
|
|
continue;
|
|
}
|
|
// The confirmed empty view repeatedly produces boxes running nearly
|
|
// from the top edge to the bottom edge. Earlier real-person bench boxes
|
|
// did not have this geometry; validate again at the installed distance.
|
|
if (box_height >= image.height * 95 / 100 && result.box[1] <= 2) {
|
|
++rejected_full_height;
|
|
continue;
|
|
}
|
|
if (!best || result.score > best->score) best = &result;
|
|
}
|
|
const int center_x = best ? (best->box[0] + best->box[2]) / 2 : 0;
|
|
const int center_y = best ? (best->box[1] + best->box[3]) / 2 : 0;
|
|
const tracking::Decision target = target_gate.update(
|
|
best != nullptr, center_x, center_y,
|
|
static_cast<uint32_t>(completed / 1000));
|
|
const int gaze_x = (!best && !target.valid)
|
|
? 500
|
|
: std::clamp(target.x, 0, image.width - 1) * 1000 /
|
|
(image.width - 1);
|
|
const int gaze_y = (!best && !target.valid)
|
|
? 500
|
|
: std::clamp(target.y, 0, image.height - 1) * 1000 /
|
|
(image.height - 1);
|
|
send_gaze(gaze_sequence++, gaze_x, gaze_y, target.valid);
|
|
if (best) {
|
|
ESP_LOGI(kTag,
|
|
"PERSON t_ms=%lld score=%.3f cx=%d cy=%d box=%d,%d,%d,%d frame=%d,%d latency_ms=%lld gaze_valid=%d target_hold=%d rejected_full_height=%d",
|
|
static_cast<long long>(completed / 1000), best->score, center_x,
|
|
center_y, best->box[0], best->box[1], best->box[2], best->box[3],
|
|
image.width, image.height,
|
|
static_cast<long long>((completed - capture_started) / 1000),
|
|
target.valid, target.held, rejected_full_height);
|
|
} else {
|
|
ESP_LOGI(kTag, "NO_PERSON t_ms=%lld frame=%d,%d latency_ms=%lld rejected_full_frame=%d rejected_full_height=%d gaze_valid=%d target_hold=%d",
|
|
static_cast<long long>(completed / 1000), image.width, image.height,
|
|
static_cast<long long>((completed - capture_started) / 1000),
|
|
rejected_full_frame, rejected_full_height, target.valid,
|
|
target.held);
|
|
}
|
|
heap_caps_free(image.data);
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
}
|
|
}
|