731 lines
29 KiB
C
731 lines
29 KiB
C
/*
|
|
* SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
/* Wi-Fi CSI console Example
|
|
|
|
This example code is in the Public Domain (or CC0 licensed, at your option.)
|
|
|
|
Unless required by applicable law or agreed to in writing, this
|
|
software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
|
|
CONDITIONS OF ANY KIND, either express or implied.
|
|
*/
|
|
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/event_groups.h"
|
|
#include "esp_log.h"
|
|
#include "nvs_flash.h"
|
|
#include "esp_err.h"
|
|
#include "esp_console.h"
|
|
|
|
#include "esp_mac.h"
|
|
#include "esp_wifi.h"
|
|
#include "lwip/inet.h"
|
|
#include "lwip/netdb.h"
|
|
#include "lwip/sockets.h"
|
|
#include "ping/ping_sock.h"
|
|
#include "hal/uart_ll.h"
|
|
#include "mbedtls/base64.h"
|
|
|
|
#include "led_strip.h"
|
|
#include "esp_radar.h"
|
|
#include "csi_commands.h"
|
|
|
|
extern esp_ping_handle_t g_ping_handle;
|
|
static led_strip_handle_t led_strip;
|
|
#if CONFIG_IDF_TARGET_ESP32C5
|
|
#define WS2812_GPIO 27
|
|
#elif CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32C61
|
|
#define WS2812_GPIO 8
|
|
#elif CONFIG_IDF_TARGET_ESP32S3
|
|
#define WS2812_GPIO 38
|
|
#elif CONFIG_IDF_TARGET_ESP32S2
|
|
#define WS2812_GPIO 18
|
|
#elif CONFIG_IDF_TARGET_ESP32C3
|
|
#define WS2812_GPIO 8
|
|
#else
|
|
#define WS2812_GPIO 4
|
|
#endif
|
|
#define RECV_ESPNOW_CSI
|
|
#define CONFIG_LESS_INTERFERENCE_CHANNEL 11
|
|
#define CONFIG_SEND_DATA_FREQUENCY 100
|
|
|
|
#define RADAR_EVALUATE_SERVER_PORT 3232
|
|
#define RADAR_BUFF_MAX_LEN 25
|
|
|
|
static QueueHandle_t g_csi_info_queue = NULL;
|
|
static bool g_wifi_connect_status = false;
|
|
static uint32_t g_send_data_interval = 1000 / CONFIG_SEND_DATA_FREQUENCY;
|
|
static const char *TAG = "app_main";
|
|
|
|
static struct {
|
|
struct arg_lit *train_start;
|
|
struct arg_lit *train_stop;
|
|
struct arg_lit *train_add;
|
|
struct arg_str *predict_someone_threshold;
|
|
struct arg_str *predict_someone_sensitivity;
|
|
struct arg_str *predict_move_threshold;
|
|
struct arg_str *predict_move_sensitivity;
|
|
struct arg_int *predict_buff_size;
|
|
struct arg_int *predict_outliers_number;
|
|
struct arg_str *collect_taget;
|
|
struct arg_int *collect_number;
|
|
struct arg_int *collect_duration;
|
|
struct arg_lit *csi_start;
|
|
struct arg_lit *csi_stop;
|
|
struct arg_str *csi_output_type;
|
|
struct arg_str *csi_output_format;
|
|
struct arg_int *csi_scale_shift;
|
|
struct arg_int *channel_filter;
|
|
struct arg_int *send_data_interval;
|
|
struct arg_end *end;
|
|
} radar_args;
|
|
static struct console_input_config {
|
|
bool train_start;
|
|
float predict_someone_threshold;
|
|
float predict_someone_sensitivity;
|
|
float predict_move_threshold;
|
|
float predict_move_sensitivity;
|
|
uint32_t predict_buff_size;
|
|
uint32_t predict_outliers_number;
|
|
char collect_taget[16];
|
|
uint32_t collect_number;
|
|
char csi_output_type[16];
|
|
char csi_output_format[16];
|
|
} g_console_input_config = {
|
|
.predict_someone_threshold = 0,
|
|
.predict_someone_sensitivity = 0.15,
|
|
.predict_move_threshold = 0.0003,
|
|
.predict_move_sensitivity = 0.20,
|
|
.predict_buff_size = 5,
|
|
.predict_outliers_number = 2,
|
|
.train_start = false,
|
|
.collect_taget = "unknown",
|
|
.csi_output_type = "LLTF",
|
|
.csi_output_format = "decimal"
|
|
};
|
|
|
|
static TimerHandle_t g_collect_timer_handele = NULL;
|
|
|
|
void wifi_csi_raw_cb(void *ctx, const wifi_csi_filtered_info_t *info)
|
|
{
|
|
wifi_csi_filtered_info_t *q_data = malloc(sizeof(wifi_csi_filtered_info_t) + info->valid_len);
|
|
*q_data = *info;
|
|
memcpy(q_data->valid_data, info->valid_data, info->valid_len);
|
|
|
|
if (!g_csi_info_queue || xQueueSend(g_csi_info_queue, &q_data, 0) == pdFALSE) {
|
|
ESP_LOGW(TAG, "g_csi_info_queue full");
|
|
free(q_data);
|
|
}
|
|
}
|
|
|
|
static void collect_timercb(TimerHandle_t timer)
|
|
{
|
|
g_console_input_config.collect_number--;
|
|
|
|
if (!g_console_input_config.collect_number) {
|
|
xTimerStop(g_collect_timer_handele, 0);
|
|
xTimerDelete(g_collect_timer_handele, 0);
|
|
g_collect_timer_handele = NULL;
|
|
strcpy(g_console_input_config.collect_taget, "unknown");
|
|
return;
|
|
}
|
|
}
|
|
|
|
static int wifi_cmd_radar(int argc, char **argv)
|
|
{
|
|
if (arg_parse(argc, argv, (void **) &radar_args) != ESP_OK) {
|
|
arg_print_errors(stderr, radar_args.end, argv[0]);
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
if (radar_args.train_start->count) {
|
|
if (!radar_args.train_add->count) {
|
|
esp_radar_train_remove();
|
|
}
|
|
|
|
esp_radar_train_start();
|
|
g_console_input_config.train_start = true;
|
|
}
|
|
|
|
if (radar_args.train_stop->count) {
|
|
esp_radar_train_stop(&g_console_input_config.predict_someone_threshold,
|
|
&g_console_input_config.predict_move_threshold);
|
|
g_console_input_config.train_start = false;
|
|
|
|
printf("RADAR_DADA,0,0,0,%.6f,0,0,%.6f,0\n",
|
|
g_console_input_config.predict_someone_threshold,
|
|
g_console_input_config.predict_move_threshold);
|
|
}
|
|
|
|
if (radar_args.predict_move_threshold->count) {
|
|
g_console_input_config.predict_move_threshold = atof(radar_args.predict_move_threshold->sval[0]);
|
|
}
|
|
|
|
if (radar_args.predict_move_sensitivity->count) {
|
|
g_console_input_config.predict_move_sensitivity = atof(radar_args.predict_move_sensitivity->sval[0]);
|
|
ESP_LOGI(TAG, "predict_move_sensitivity: %f", g_console_input_config.predict_move_sensitivity);
|
|
}
|
|
|
|
if (radar_args.predict_someone_threshold->count) {
|
|
g_console_input_config.predict_someone_threshold = atof(radar_args.predict_someone_threshold->sval[0]);
|
|
}
|
|
|
|
if (radar_args.predict_someone_sensitivity->count) {
|
|
g_console_input_config.predict_someone_sensitivity = atof(radar_args.predict_someone_sensitivity->sval[0]);
|
|
ESP_LOGI(TAG, "predict_someone_sensitivity: %f", g_console_input_config.predict_someone_sensitivity);
|
|
}
|
|
|
|
if (radar_args.predict_buff_size->count) {
|
|
g_console_input_config.predict_buff_size = radar_args.predict_buff_size->ival[0];
|
|
}
|
|
|
|
if (radar_args.predict_outliers_number->count) {
|
|
g_console_input_config.predict_outliers_number = radar_args.predict_outliers_number->ival[0];
|
|
}
|
|
|
|
if (radar_args.collect_taget->count && radar_args.collect_number->count && radar_args.collect_duration->count) {
|
|
g_console_input_config.collect_number = radar_args.collect_number->ival[0];
|
|
strcpy(g_console_input_config.collect_taget, radar_args.collect_taget->sval[0]);
|
|
|
|
if (g_collect_timer_handele) {
|
|
xTimerStop(g_collect_timer_handele, portMAX_DELAY);
|
|
xTimerDelete(g_collect_timer_handele, portMAX_DELAY);
|
|
}
|
|
|
|
g_collect_timer_handele = xTimerCreate("collect", pdMS_TO_TICKS(radar_args.collect_duration->ival[0]),
|
|
true, NULL, collect_timercb);
|
|
xTimerStart(g_collect_timer_handele, portMAX_DELAY);
|
|
}
|
|
|
|
if (radar_args.csi_output_format->count) {
|
|
strcpy(g_console_input_config.csi_output_format, radar_args.csi_output_format->sval[0]);
|
|
}
|
|
|
|
if (radar_args.csi_output_type->count) {
|
|
esp_radar_config_t radar_config = {0};
|
|
esp_radar_get_config(&radar_config);
|
|
|
|
if (!strcasecmp(radar_args.csi_output_type->sval[0], "NULL")) {
|
|
radar_config.csi_config.csi_filtered_cb = NULL;
|
|
} else {
|
|
radar_config.csi_config.csi_filtered_cb = wifi_csi_raw_cb;
|
|
strcpy(g_console_input_config.csi_output_type, radar_args.csi_output_type->sval[0]);
|
|
#if CONFIG_IDF_TARGET_ESP32C5 || CONFIG_IDF_TARGET_ESP32C61
|
|
if (!strcasecmp(radar_args.csi_output_type->sval[0], "LLTF")) {
|
|
radar_config.csi_config.acquire_csi_lltf = true;
|
|
radar_config.dec_config.ltf_type = RADAR_LTF_TYPE_LLTF;
|
|
radar_config.dec_config.sub_carrier_step_size = 2;
|
|
} else if (!strcasecmp(radar_args.csi_output_type->sval[0], "HT-LTF")) {
|
|
radar_config.csi_config.acquire_csi_lltf = false;
|
|
radar_config.csi_config.acquire_csi_ht20 = true;
|
|
radar_config.csi_config.acquire_csi_ht40 = true;
|
|
radar_config.csi_config.acquire_csi_vht = true;
|
|
radar_config.dec_config.ltf_type = RADAR_LTF_TYPE_HTLTF;
|
|
radar_config.dec_config.sub_carrier_step_size = 5;
|
|
} else if (!strcasecmp(radar_args.csi_output_type->sval[0], "STBC-HT-LTF")) {
|
|
radar_config.csi_config.acquire_csi_lltf = false;
|
|
radar_config.csi_config.acquire_csi_ht20 = true;
|
|
radar_config.csi_config.acquire_csi_ht40 = true;
|
|
radar_config.csi_config.acquire_csi_vht = true;
|
|
radar_config.dec_config.ltf_type = RADAR_LTF_TYPE_STBC_HTLTF;
|
|
radar_config.dec_config.sub_carrier_step_size = 5;
|
|
} else if (!strcasecmp(radar_args.csi_output_type->sval[0], "HE-LTF")) {
|
|
radar_config.csi_config.acquire_csi_lltf = false;
|
|
radar_config.csi_config.acquire_csi_su = true;
|
|
radar_config.csi_config.acquire_csi_mu = true;
|
|
radar_config.csi_config.acquire_csi_dcm = true;
|
|
radar_config.csi_config.acquire_csi_beamformed = true;
|
|
radar_config.csi_config.acquire_csi_vht = true;
|
|
radar_config.dec_config.ltf_type = RADAR_LTF_TYPE_HELTF;
|
|
radar_config.dec_config.sub_carrier_step_size = 5;
|
|
} else if (!strcasecmp(radar_args.csi_output_type->sval[0], "STBC-HE-LTF")) {
|
|
radar_config.csi_config.acquire_csi_lltf = false;
|
|
radar_config.csi_config.acquire_csi_su = true;
|
|
radar_config.csi_config.acquire_csi_mu = true;
|
|
radar_config.csi_config.acquire_csi_dcm = true;
|
|
radar_config.csi_config.acquire_csi_beamformed = true;
|
|
radar_config.csi_config.acquire_csi_he_stbc_mode = CSI_HE_STBC_MODE_AVERAGE;
|
|
radar_config.csi_config.acquire_csi_vht = true;
|
|
radar_config.dec_config.ltf_type = RADAR_LTF_TYPE_STBC_HELTF;
|
|
radar_config.dec_config.sub_carrier_step_size = 5;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
esp_radar_change_config(&radar_config);
|
|
}
|
|
|
|
if (radar_args.csi_start->count) {
|
|
esp_radar_start();
|
|
}
|
|
|
|
if (radar_args.csi_stop->count) {
|
|
esp_radar_stop();
|
|
}
|
|
|
|
#if CONFIG_IDF_TARGET_ESP32S3 || CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32C3 || CONFIG_IDF_TARGET_ESP32
|
|
if (radar_args.csi_scale_shift->count) {
|
|
esp_radar_config_t radar_config = {0};
|
|
esp_radar_get_config(&radar_config);
|
|
radar_config.csi_config.shift = radar_args.csi_scale_shift->ival[0];
|
|
esp_radar_change_config(&radar_config);
|
|
|
|
ESP_LOGI(TAG, "manually left shift %d bits of the scale of the CSI data", radar_config.csi_config.shift);
|
|
}
|
|
|
|
if (radar_args.channel_filter->count) {
|
|
esp_radar_config_t radar_config = {0};
|
|
esp_radar_get_config(&radar_config);
|
|
radar_config.csi_config.channel_filter_en = radar_args.channel_filter->ival[0];
|
|
esp_radar_change_config(&radar_config);
|
|
|
|
ESP_LOGI(TAG, "enable(%d) to turn on channel filter to smooth adjacent sub-carrier", radar_config.csi_config.channel_filter_en);
|
|
}
|
|
#endif
|
|
if (radar_args.send_data_interval->count) {
|
|
g_send_data_interval = radar_args.send_data_interval->ival[0];
|
|
}
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
void cmd_register_radar(void)
|
|
{
|
|
radar_args.train_start = arg_lit0(NULL, "train_start", "Start calibrating the 'Radar' algorithm");
|
|
radar_args.train_stop = arg_lit0(NULL, "train_stop", "Stop calibrating the 'Radar' algorithm");
|
|
radar_args.train_add = arg_lit0(NULL, "train_add", "Calibrate on the basis of saving the calibration results");
|
|
|
|
radar_args.predict_someone_threshold = arg_str0(NULL, "predict_someone_threshold", "<0 ~ 1.0>", "Configure the threshold for someone");
|
|
radar_args.predict_someone_sensitivity = arg_str0(NULL, "predict_someone_sensitivity", "<0 ~ 1.0>", "Configure the sensitivity for someone");
|
|
radar_args.predict_move_threshold = arg_str0(NULL, "predict_move_threshold", "<0 ~ 1.0>", "Configure the threshold for move");
|
|
radar_args.predict_move_sensitivity = arg_str0(NULL, "predict_move_sensitivity", "<0 ~ 1.0>", "Configure the sensitivity for move");
|
|
radar_args.predict_buff_size = arg_int0(NULL, "predict_buff_size", "1 ~ 100", "Buffer size for filtering outliers");
|
|
radar_args.predict_outliers_number = arg_int0(NULL, "predict_outliers_number", "<1 ~ 100>", "The number of items in the buffer queue greater than the threshold");
|
|
|
|
radar_args.collect_taget = arg_str0(NULL, "collect_tagets", "<0 ~ 20>", "Type of CSI data collected");
|
|
radar_args.collect_number = arg_int0(NULL, "collect_number", "sequence", "Number of times CSI data was collected");
|
|
radar_args.collect_duration = arg_int0(NULL, "collect_duration", "duration", "Time taken to acquire one CSI data");
|
|
|
|
radar_args.csi_start = arg_lit0(NULL, "csi_start", "Start collecting CSI data from Wi-Fi");
|
|
radar_args.csi_stop = arg_lit0(NULL, "csi_stop", "Stop CSI data collection from Wi-Fi");
|
|
radar_args.csi_output_type = arg_str0(NULL, "csi_output_type", "<NULL, LLTF, HT-LTF, HE-LTF, STBC-HT-LTF, STBC-HE-LTF>", "Type of CSI data");
|
|
radar_args.csi_output_format = arg_str0(NULL, "csi_output_format", "<decimal, base64>", "Format of CSI data");
|
|
radar_args.csi_scale_shift = arg_int0(NULL, "scale_shift", "<0~15>", "manually left shift bits of the scale of the CSI data");
|
|
radar_args.channel_filter = arg_int0(NULL, "channel_filter", "<0 or 1>", "enable to turn on channel filter to smooth adjacent sub-carrier");
|
|
|
|
radar_args.send_data_interval = arg_int0(NULL, "send_data_interval", "<interval_ms>", "The interval between sending null data or ping packets to the router");
|
|
|
|
radar_args.end = arg_end(8);
|
|
|
|
const esp_console_cmd_t radar_cmd = {
|
|
.command = "radar",
|
|
.help = "Radar config",
|
|
.hint = NULL,
|
|
.func = &wifi_cmd_radar,
|
|
.argtable = &radar_args
|
|
};
|
|
|
|
ESP_ERROR_CHECK(esp_console_cmd_register(&radar_cmd));
|
|
}
|
|
|
|
static void csi_data_print_task(void *arg)
|
|
{
|
|
wifi_csi_filtered_info_t *info = NULL;
|
|
char *buffer = malloc(8 * 1024);
|
|
static uint32_t count = 0;
|
|
|
|
while (xQueueReceive(g_csi_info_queue, &info, portMAX_DELAY)) {
|
|
size_t len = 0;
|
|
esp_radar_rx_ctrl_info_t *rx_ctrl = &info->rx_ctrl_info;
|
|
|
|
if (!count) {
|
|
ESP_LOGI(TAG, "================ CSI RECV ================");
|
|
len += sprintf(buffer + len, "type,sequence,timestamp,taget_seq,target,mac,rssi,rate,sig_mode,mcs,bandwidth,smoothing,not_sounding,aggregation,stbc,fec_coding,sgi,noise_floor,ampdu_cnt,channel,secondary_channel,local_timestamp,ant,sig_len,rx_state,agc_gain,fft_gain,len,first_word,data\n");
|
|
}
|
|
uint16_t valid_len = info->valid_len;
|
|
ESP_LOGI(TAG, "info->valid_len1: %d", info->valid_len);
|
|
if (!strcasecmp(g_console_input_config.csi_output_type, "LLTF")) {
|
|
info->valid_len = info->valid_lltf_len;
|
|
} else if (!strcasecmp(g_console_input_config.csi_output_type, "HT-LTF")) {
|
|
info->valid_len = info->valid_lltf_len + info->valid_ht_ltf_len;
|
|
} else if (!strcasecmp(g_console_input_config.csi_output_type, "STBC-HT-LTF")) {
|
|
info->valid_len = info->valid_lltf_len + info->valid_ht_ltf_len + info->valid_stbc_ht_ltf_len;
|
|
#if CONFIG_IDF_TARGET_ESP32C5 || CONFIG_IDF_TARGET_ESP32C6 || CONFIG_IDF_TARGET_ESP32C61
|
|
} else if (!strcasecmp(g_console_input_config.csi_output_type, "HE-LTF")) {
|
|
info->valid_len = info->valid_he_ltf_len;
|
|
} else if (!strcasecmp(g_console_input_config.csi_output_type, "STBC-HE-LTF")) {
|
|
info->valid_len = info->valid_he_ltf_len + info->valid_stbc_he_ltf_len;
|
|
#endif
|
|
}
|
|
ESP_LOGI(TAG, "info->valid_len: %d", info->valid_len);
|
|
if (info->valid_len == 0) {
|
|
info->valid_len = valid_len;
|
|
|
|
}
|
|
len += sprintf(buffer + len, "CSI_DATA,%s,%s,%d,%u,%u,%s," MACSTR ",%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%u,%d,%d,%d,%d,%d,%d,%d,",
|
|
CONFIG_DEVICE_NAME, CONFIG_DEVICE_LOCATION,
|
|
count++, esp_log_timestamp(), g_console_input_config.collect_number, g_console_input_config.collect_taget,
|
|
MAC2STR(info->mac), rx_ctrl->rssi, rx_ctrl->rate, rx_ctrl->signal_mode,
|
|
rx_ctrl->mcs, rx_ctrl->cwb, 0, 0,
|
|
0, rx_ctrl->stbc, 0, 0,
|
|
rx_ctrl->noise_floor, 0, rx_ctrl->channel, rx_ctrl->secondary_channel,
|
|
rx_ctrl->timestamp, 0, 0, 0, rx_ctrl->agc_gain, rx_ctrl->fft_gain, info->valid_len, 0);
|
|
|
|
if (!strcasecmp(g_console_input_config.csi_output_format, "base64")) {
|
|
size_t size = 0;
|
|
mbedtls_base64_encode((uint8_t *)buffer + len, sizeof(buffer) - len, &size, (uint8_t *)info->valid_data, info->valid_len);
|
|
len += size;
|
|
len += sprintf(buffer + len, "\n");
|
|
} else {
|
|
len += sprintf(buffer + len, "\"[%d", info->valid_data[0]);
|
|
|
|
for (int i = 1; i < info->valid_len; i++) {
|
|
len += sprintf(buffer + len, ",%d", info->valid_data[i]);
|
|
}
|
|
|
|
len += sprintf(buffer + len, "]\"\n");
|
|
}
|
|
|
|
printf("%s", buffer);
|
|
free(info);
|
|
}
|
|
|
|
free(buffer);
|
|
vTaskDelete(NULL);
|
|
}
|
|
|
|
static void wifi_radar_cb(void *ctx, const wifi_radar_info_t *info)
|
|
{
|
|
static float *s_buff_wander = NULL;
|
|
static float *s_buff_jitter = NULL;
|
|
static uint32_t s_buff_count = 0;
|
|
uint32_t buff_max_size = g_console_input_config.predict_buff_size;
|
|
uint32_t buff_outliers_num = g_console_input_config.predict_outliers_number;
|
|
uint32_t someone_count = 0;
|
|
uint32_t move_count = 0;
|
|
bool room_status = false;
|
|
bool human_status = false;
|
|
|
|
if (!s_buff_wander) {
|
|
s_buff_wander = calloc(RADAR_BUFF_MAX_LEN, sizeof(float));
|
|
}
|
|
|
|
if (!s_buff_jitter) {
|
|
s_buff_jitter = calloc(RADAR_BUFF_MAX_LEN, sizeof(float));
|
|
}
|
|
|
|
s_buff_wander[s_buff_count % RADAR_BUFF_MAX_LEN] = info->waveform_wander;
|
|
s_buff_jitter[s_buff_count % RADAR_BUFF_MAX_LEN] = info->waveform_jitter;
|
|
s_buff_count++;
|
|
|
|
if (s_buff_count < buff_max_size) {
|
|
return;
|
|
}
|
|
|
|
extern float trimmean(const float * array, size_t len, float percent);
|
|
extern float median(const float * a, size_t len);
|
|
|
|
float wander_average = trimmean(s_buff_wander, RADAR_BUFF_MAX_LEN, 0.5);
|
|
float jitter_midean = median(s_buff_jitter, RADAR_BUFF_MAX_LEN);
|
|
|
|
for (int i = 0; i < buff_max_size; i++) {
|
|
uint32_t index = (s_buff_count - 1 - i) % RADAR_BUFF_MAX_LEN;
|
|
|
|
if ((wander_average * g_console_input_config.predict_someone_sensitivity > g_console_input_config.predict_someone_threshold)) {
|
|
someone_count++;
|
|
}
|
|
|
|
if (s_buff_jitter[index] * g_console_input_config.predict_move_sensitivity > g_console_input_config.predict_move_threshold
|
|
|| (s_buff_jitter[index] * g_console_input_config.predict_move_sensitivity > jitter_midean && s_buff_jitter[index] > 0.0002)) {
|
|
move_count++;
|
|
}
|
|
}
|
|
|
|
if (someone_count >= 1) {
|
|
room_status = true;
|
|
}
|
|
|
|
if (move_count >= buff_outliers_num) {
|
|
human_status = true;
|
|
}
|
|
|
|
static uint32_t s_count = 0;
|
|
|
|
if (!s_count) {
|
|
ESP_LOGI(TAG, "================ RADAR RECV ================");
|
|
ESP_LOGI(TAG, "type,sequence,timestamp,waveform_wander,someone_threshold,someone_status,waveform_jitter,move_threshold,move_status");
|
|
}
|
|
|
|
char timestamp_str[32] = {0};
|
|
sprintf(timestamp_str, "%u", esp_log_timestamp());
|
|
|
|
if (ctx) {
|
|
strncpy(timestamp_str, (char *)ctx, 31);
|
|
}
|
|
|
|
static uint32_t s_last_move_time = 0;
|
|
static uint32_t s_last_someone_time = 0;
|
|
|
|
if (g_console_input_config.train_start) {
|
|
s_last_move_time = esp_log_timestamp();
|
|
s_last_someone_time = esp_log_timestamp();
|
|
|
|
static bool led_status = false;
|
|
|
|
if (led_status) {
|
|
led_strip_set_pixel(led_strip, 0, 0, 0, 0);
|
|
} else {
|
|
led_strip_set_pixel(led_strip, 0, 255, 255, 0);
|
|
}
|
|
led_status = !led_status;
|
|
led_strip_refresh(led_strip);
|
|
return;
|
|
}
|
|
|
|
printf("RADAR_DADA,%d,%s,%.6f,%.6f,%.6f,%d,%.6f,%.6f,%.6f,%d\n",
|
|
s_count++, timestamp_str,
|
|
info->waveform_wander, wander_average, g_console_input_config.predict_someone_threshold / g_console_input_config.predict_someone_sensitivity, room_status,
|
|
info->waveform_jitter, jitter_midean, jitter_midean / g_console_input_config.predict_move_sensitivity, human_status);
|
|
|
|
if (room_status) {
|
|
if (human_status) {
|
|
led_strip_set_pixel(led_strip, 0, 0, 255, 0);
|
|
ESP_LOGI(TAG, "Someone moved");
|
|
s_last_move_time = esp_log_timestamp();
|
|
} else if (esp_log_timestamp() - s_last_move_time > 3 * 1000) {
|
|
led_strip_set_pixel(led_strip, 0, 255, 255, 255);
|
|
ESP_LOGI(TAG, "Someone");
|
|
}
|
|
|
|
s_last_someone_time = esp_log_timestamp();
|
|
} else if (esp_log_timestamp() - s_last_someone_time > 3 * 1000) {
|
|
if (human_status) {
|
|
s_last_move_time = esp_log_timestamp();
|
|
led_strip_set_pixel(led_strip, 0, 255, 0, 0);
|
|
} else if (esp_log_timestamp() - s_last_move_time > 3 * 1000) {
|
|
led_strip_set_pixel(led_strip, 0, 0, 0, 0);
|
|
}
|
|
}
|
|
led_strip_refresh(led_strip);
|
|
}
|
|
|
|
static void trigger_router_send_data_task(void *arg)
|
|
{
|
|
esp_radar_config_t radar_config = {0};
|
|
wifi_ap_record_t ap_info = {0};
|
|
uint8_t sta_mac[6] = {0};
|
|
|
|
esp_radar_get_config(&radar_config);
|
|
esp_wifi_sta_get_ap_info(&ap_info);
|
|
ESP_ERROR_CHECK(esp_wifi_get_mac(WIFI_IF_STA, sta_mac));
|
|
|
|
radar_config.csi_config.csi_recv_interval = g_send_data_interval;
|
|
memcpy(radar_config.csi_config.filter_dmac, sta_mac, sizeof(radar_config.csi_config.filter_dmac));
|
|
|
|
#if WIFI_CSI_SEND_NULL_DATA_ENABLE
|
|
ESP_LOGI(TAG, "Send null data to router");
|
|
|
|
memset(radar_config.csi_config.filter_mac, 0, sizeof(radar_config.csi_config.filter_mac));
|
|
esp_radar_change_config(&radar_config);
|
|
|
|
typedef struct {
|
|
uint8_t frame_control[2];
|
|
uint16_t duration;
|
|
uint8_t destination_address[6];
|
|
uint8_t source_address[6];
|
|
uint8_t broadcast_address[6];
|
|
uint16_t sequence_control;
|
|
} __attribute__((packed)) wifi_null_data_t;
|
|
|
|
wifi_null_data_t null_data = {
|
|
.frame_control = {0x48, 0x01},
|
|
.duration = 0x0000,
|
|
.sequence_control = 0x0000,
|
|
};
|
|
|
|
memcpy(null_data.destination_address, ap_info.bssid, 6);
|
|
memcpy(null_data.broadcast_address, ap_info.bssid, 6);
|
|
memcpy(null_data.source_address, sta_mac, 6);
|
|
|
|
ESP_LOGW(TAG, "null_data, destination_address: "MACSTR", source_address: "MACSTR", broadcast_address: " MACSTR,
|
|
MAC2STR(null_data.destination_address), MAC2STR(null_data.source_address), MAC2STR(null_data.broadcast_address));
|
|
|
|
ESP_ERROR_CHECK(esp_wifi_config_80211_tx_rate(WIFI_IF_STA, WIFI_PHY_RATE_6M));
|
|
|
|
for (int i = 0; g_wifi_connect_status; i++) {
|
|
esp_err_t ret = esp_wifi_80211_tx(WIFI_IF_STA, &null_data, sizeof(wifi_null_data_t), true);
|
|
if (ret != ESP_OK) {
|
|
ESP_LOGW(TAG, "esp_wifi_80211_tx, %s", esp_err_to_name(ret));
|
|
vTaskDelay(pdMS_TO_TICKS(1000));
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(g_send_data_interval));
|
|
}
|
|
|
|
#else
|
|
ESP_LOGI(TAG, "Send ping data to router");
|
|
|
|
memcpy(radar_config.csi_config.filter_mac, ap_info.bssid, sizeof(radar_config.csi_config.filter_mac));
|
|
esp_radar_change_config(&radar_config);
|
|
|
|
/* stop and delete existing ping session if any */
|
|
if (g_ping_handle != NULL) {
|
|
ESP_LOGI(TAG, "Stopping existing ping session before starting new one");
|
|
esp_ping_stop(g_ping_handle);
|
|
esp_ping_delete_session(g_ping_handle);
|
|
g_ping_handle = NULL;
|
|
ESP_LOGI(TAG, "Existing ping session stopped and deleted");
|
|
}
|
|
|
|
esp_ping_config_t config = ESP_PING_DEFAULT_CONFIG();
|
|
config.count = 0;
|
|
config.data_size = 1;
|
|
config.interval_ms = g_send_data_interval;
|
|
|
|
/**
|
|
* @brief Get the Router IP information from the esp-netif
|
|
*/
|
|
esp_netif_ip_info_t local_ip;
|
|
esp_netif_get_ip_info(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"), &local_ip);
|
|
ESP_LOGI(TAG, "Ping: got ip:" IPSTR ", gw: " IPSTR, IP2STR(&local_ip.ip), IP2STR(&local_ip.gw));
|
|
config.target_addr.u_addr.ip4.addr = ip4_addr_get_u32(&local_ip.gw);
|
|
config.target_addr.type = ESP_IPADDR_TYPE_V4;
|
|
|
|
esp_ping_callbacks_t cbs = { 0 };
|
|
esp_ping_new_session(&config, &cbs, &g_ping_handle);
|
|
esp_ping_start(g_ping_handle);
|
|
#endif
|
|
|
|
vTaskDelete(NULL);
|
|
}
|
|
|
|
/* Event handler for catching system events */
|
|
static void wifi_event_handler(void *arg, esp_event_base_t event_base,
|
|
int32_t event_id, void *event_data)
|
|
{
|
|
if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
|
|
g_wifi_connect_status = true;
|
|
|
|
xTaskCreate(trigger_router_send_data_task, "trigger_router_send_data", 4 * 1024, NULL, 5, NULL);
|
|
|
|
#ifdef RECV_ESPNOW_CSI
|
|
ESP_ERROR_CHECK(esp_wifi_set_promiscuous(false));
|
|
#endif
|
|
} else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
|
|
g_wifi_connect_status = false;
|
|
ESP_LOGW(TAG, "Wi-Fi disconnected");
|
|
esp_radar_config_t radar_config;
|
|
esp_radar_get_config(&radar_config);
|
|
esp_radar_wifi_reinit(&radar_config.wifi_config);
|
|
|
|
}
|
|
}
|
|
esp_err_t ws2812_led_init(void)
|
|
{
|
|
led_strip_config_t strip_config = {
|
|
.strip_gpio_num = WS2812_GPIO,
|
|
.max_leds = 1,
|
|
};
|
|
|
|
led_strip_rmt_config_t rmt_config = {
|
|
.clk_src = RMT_CLK_SRC_DEFAULT, // different clock source can lead to different power consumption
|
|
.resolution_hz = 10 * 1000 * 1000, // RMT counter clock frequency: 10MHz
|
|
.flags = {
|
|
.with_dma = false, // DMA feature is available on chips like ESP32-S3/P4
|
|
}
|
|
};
|
|
ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_config, &rmt_config, &led_strip));
|
|
/* Set all LED off to clear all pixels */
|
|
led_strip_clear(led_strip);
|
|
return ESP_OK;
|
|
}
|
|
void app_main(void)
|
|
{
|
|
ESP_LOGI(TAG, "app_main start, line: %d", __LINE__);
|
|
/**
|
|
* @brief Initialize NVS
|
|
*/
|
|
|
|
esp_err_t ret = nvs_flash_init();
|
|
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
|
|
ESP_ERROR_CHECK(nvs_flash_erase());
|
|
ret = nvs_flash_init();
|
|
}
|
|
ESP_ERROR_CHECK(ret);
|
|
ESP_LOGI(TAG, "app_main line: %d", __LINE__);
|
|
/**
|
|
* @brief Install ws2812 driver, Used to display the status of the device
|
|
*/
|
|
ws2812_led_init();
|
|
|
|
ESP_LOGI(TAG, "app_main start, line: %d", __LINE__);
|
|
/**
|
|
* @brief Turn on the radar module printing information
|
|
*/
|
|
esp_log_level_set("esp_radar", ESP_LOG_INFO);
|
|
|
|
/**
|
|
* @brief Register serial command
|
|
*/
|
|
esp_console_repl_t *repl = NULL;
|
|
esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT();
|
|
esp_console_dev_uart_config_t uart_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT();
|
|
repl_config.prompt = "csi>";
|
|
ESP_ERROR_CHECK(esp_console_new_repl_uart(&uart_config, &repl_config, &repl));
|
|
|
|
#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2
|
|
/**< Fix serial port garbled code due to high baud rate */
|
|
uart_ll_set_sclk(UART_LL_GET_HW(CONFIG_ESP_CONSOLE_UART_NUM), UART_SCLK_APB);
|
|
#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 0)
|
|
uart_ll_set_baudrate(UART_LL_GET_HW(CONFIG_ESP_CONSOLE_UART_NUM), CONFIG_ESP_CONSOLE_UART_BAUDRATE, APB_CLK_FREQ);
|
|
#else
|
|
uart_ll_set_baudrate(UART_LL_GET_HW(CONFIG_ESP_CONSOLE_UART_NUM), CONFIG_ESP_CONSOLE_UART_BAUDRATE);
|
|
#endif
|
|
#endif
|
|
|
|
/**
|
|
* @brief Set the Wi-Fi radar configuration
|
|
*/
|
|
esp_radar_csi_config_t csi_config = ESP_RADAR_CSI_CONFIG_DEFAULT();
|
|
esp_radar_wifi_config_t wifi_config = ESP_RADAR_WIFI_CONFIG_DEFAULT();
|
|
esp_radar_espnow_config_t espnow_config = ESP_RADAR_ESPNOW_CONFIG_DEFAULT();
|
|
esp_radar_dec_config_t dec_config = ESP_RADAR_DEC_CONFIG_DEFAULT();
|
|
memcpy(csi_config.filter_mac, "\x1a\x00\x00\x00\x00\x00", 6);
|
|
csi_config.csi_recv_interval = g_send_data_interval;
|
|
dec_config.wifi_radar_cb = wifi_radar_cb;
|
|
#if WIFI_CSI_SEND_NULL_DATA_ENABLE
|
|
csi_config.dump_ack_en = true;
|
|
#endif
|
|
dec_config.outliers_threshold = 0;
|
|
ESP_ERROR_CHECK(esp_radar_wifi_init(&wifi_config));
|
|
ESP_ERROR_CHECK(esp_radar_csi_init(&csi_config));
|
|
ESP_ERROR_CHECK(esp_radar_dec_init(&dec_config));
|
|
|
|
ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL));
|
|
ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT, WIFI_EVENT_STA_DISCONNECTED, &wifi_event_handler, NULL));
|
|
|
|
cmd_register_ping();
|
|
cmd_register_system();
|
|
cmd_register_wifi_config();
|
|
cmd_register_wifi_scan();
|
|
cmd_register_radar();
|
|
ESP_ERROR_CHECK(esp_console_start_repl(repl));
|
|
|
|
/**
|
|
* @brief Start Wi-Fi radar
|
|
*/
|
|
esp_radar_start();
|
|
|
|
/**
|
|
* @brief Initialize CSI serial port printing task, Use tasks to avoid blocking wifi_csi_raw_cb
|
|
*/
|
|
g_csi_info_queue = xQueueCreate(64, sizeof(void *));
|
|
xTaskCreate(csi_data_print_task, "csi_data_print", 4 * 1024, NULL, 0, NULL);
|
|
}
|