/** * @file mod_mlat.c * @brief Multilateration Scanner Module (BLE + WiFi) * * This module turns an ESP32 into an RSSI scanner for multilateration. * Supports both BLE and WiFi modes, switchable at runtime from C2. * Position is configured from C2, and RSSI readings are sent back via TCP. * * Supports two coordinate systems: * - GPS (lat/lon in degrees) for outdoor tracking with real maps * - Local (x/y in meters) for indoor tracking with floor plans * * Commands: * mlat config gps - Set GPS position (degrees) * mlat config local - Set local position (meters) * mlat config - Backward compat: GPS mode * mlat mode - Set scanning mode * mlat start - Start scanning for target MAC * mlat stop - Stop scanning * mlat status - Show current config and state * * Data format sent to C2: * MLAT:G;;; - GPS coordinates * MLAT:L;;; - Local coordinates (meters) */ #include #include #include #include #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_log.h" #include "esp_err.h" #include "nvs_flash.h" /* BLE */ #include "esp_bt.h" #include "esp_gap_ble_api.h" #include "esp_bt_main.h" /* WiFi */ #include "esp_wifi.h" #include "esp_event.h" #include "utils.h" #if defined(CONFIG_RECON_MODE_MLAT) /* ============================================================ * CONFIG * ============================================================ */ #define TAG "MLAT" #define SEND_INTERVAL_MS 2000 /* Send aggregated RSSI every 2s */ #define RSSI_HISTORY_SIZE 10 /* Keep last N readings for averaging */ #define CHANNEL_HOP_MS 200 /* WiFi channel hop interval */ /* ============================================================ * TYPES * ============================================================ */ typedef enum { MLAT_MODE_NONE = 0, MLAT_MODE_BLE, MLAT_MODE_WIFI } mlat_mode_t; typedef enum { COORD_GPS = 0, /* lat/lon (degrees) */ COORD_LOCAL /* x/y (meters) */ } coord_type_t; /* WiFi frame header for promiscuous mode */ typedef struct { unsigned frame_ctrl:16; unsigned duration_id:16; uint8_t addr1[6]; /* Destination */ uint8_t addr2[6]; /* Source */ uint8_t addr3[6]; /* BSSID */ unsigned seq_ctrl:16; } __attribute__((packed)) wifi_mgmt_hdr_t; /* ============================================================ * STATE * ============================================================ */ static bool mlat_configured = false; static bool mlat_running = false; static mlat_mode_t mlat_mode = MLAT_MODE_BLE; /* Default to BLE */ /* Hardware init state */ static bool ble_initialized = false; static bool wifi_promisc_enabled = false; /* Scanner position (set via mlat config) */ static coord_type_t coord_type = COORD_GPS; static double scanner_lat = 0.0; /* GPS latitude (degrees) */ static double scanner_lon = 0.0; /* GPS longitude (degrees) */ static double scanner_x = 0.0; /* Local X position (meters) */ static double scanner_y = 0.0; /* Local Y position (meters) */ /* Target MAC */ static uint8_t target_mac[6] = {0}; static char target_mac_str[20] = {0}; /* RSSI history for averaging */ static int8_t rssi_history[RSSI_HISTORY_SIZE]; static size_t rssi_count = 0; static size_t rssi_index = 0; /* Task handles */ static TaskHandle_t send_task_handle = NULL; static TaskHandle_t hop_task_handle = NULL; /* WiFi current channel */ static uint8_t current_channel = 1; /* ============================================================ * UTILS * ============================================================ */ static bool parse_mac_str(const char *input, uint8_t *mac_out) { char clean[13] = {0}; int j = 0; for (int i = 0; input[i] && j < 12; i++) { char c = input[i]; if (c == ':' || c == '-' || c == ' ') continue; if (!isxdigit((unsigned char)c)) return false; clean[j++] = toupper((unsigned char)c); } if (j != 12) return false; for (int i = 0; i < 6; i++) { char b[3] = { clean[i*2], clean[i*2+1], 0 }; mac_out[i] = (uint8_t)strtol(b, NULL, 16); } return true; } static void mac_to_str(const uint8_t *mac, char *out, size_t len) { snprintf(out, len, "%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); } static int8_t get_average_rssi(void) { if (rssi_count == 0) return 0; int32_t sum = 0; size_t count = (rssi_count < RSSI_HISTORY_SIZE) ? rssi_count : RSSI_HISTORY_SIZE; for (size_t i = 0; i < count; i++) { sum += rssi_history[i]; } return (int8_t)(sum / (int32_t)count); } static void add_rssi_reading(int8_t rssi) { rssi_history[rssi_index] = rssi; rssi_index = (rssi_index + 1) % RSSI_HISTORY_SIZE; if (rssi_count < RSSI_HISTORY_SIZE) { rssi_count++; } } static void reset_rssi_history(void) { memset(rssi_history, 0, sizeof(rssi_history)); rssi_count = 0; rssi_index = 0; } static const char *mode_to_str(mlat_mode_t mode) { switch (mode) { case MLAT_MODE_BLE: return "BLE"; case MLAT_MODE_WIFI: return "WiFi"; default: return "none"; } } /* ============================================================ * BLE CALLBACK * ============================================================ */ static void ble_scan_cb(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param) { if (!mlat_running || mlat_mode != MLAT_MODE_BLE) return; if (event != ESP_GAP_BLE_SCAN_RESULT_EVT || param->scan_rst.search_evt != ESP_GAP_SEARCH_INQ_RES_EVT) return; /* Check if this is our target */ if (memcmp(param->scan_rst.bda, target_mac, 6) != 0) return; /* Store RSSI reading */ add_rssi_reading(param->scan_rst.rssi); } /* ============================================================ * WIFI PROMISCUOUS CALLBACK * ============================================================ */ static void IRAM_ATTR wifi_promisc_cb(void *buf, wifi_promiscuous_pkt_type_t type) { if (!mlat_running || mlat_mode != MLAT_MODE_WIFI) return; /* Only interested in management frames (probe requests, etc.) */ if (type != WIFI_PKT_MGMT) return; wifi_promiscuous_pkt_t *pkt = (wifi_promiscuous_pkt_t *)buf; wifi_mgmt_hdr_t *hdr = (wifi_mgmt_hdr_t *)pkt->payload; /* Check if source MAC (addr2) matches our target */ if (memcmp(hdr->addr2, target_mac, 6) != 0) return; /* Store RSSI reading */ add_rssi_reading(pkt->rx_ctrl.rssi); } /* ============================================================ * WIFI CHANNEL HOP TASK * ============================================================ */ static void channel_hop_task(void *arg) { (void)arg; while (mlat_running && mlat_mode == MLAT_MODE_WIFI) { vTaskDelay(pdMS_TO_TICKS(CHANNEL_HOP_MS)); if (!mlat_running || mlat_mode != MLAT_MODE_WIFI) break; current_channel = (current_channel % 13) + 1; esp_wifi_set_channel(current_channel, WIFI_SECOND_CHAN_NONE); } hop_task_handle = NULL; ESP_LOGI(TAG, "channel hop task stopped"); vTaskDelete(NULL); } /* ============================================================ * SEND TASK - Periodically send RSSI to C2 * ============================================================ */ static void mlat_send_task(void *arg) { (void)arg; char msg[128]; while (mlat_running) { vTaskDelay(pdMS_TO_TICKS(SEND_INTERVAL_MS)); if (!mlat_running) break; if (rssi_count > 0) { int8_t avg_rssi = get_average_rssi(); /* * Send MLAT data to C2 via msg_info * Format GPS: MLAT:G;;; * Format Local: MLAT:L;;; * The C2 will parse messages starting with "MLAT:" and extract the data */ if (coord_type == COORD_GPS) { snprintf(msg, sizeof(msg), "MLAT:G;%.6f;%.6f;%d", scanner_lat, scanner_lon, avg_rssi); ESP_LOGD(TAG, "sent: GPS=(%.6f,%.6f) rssi=%d (avg of %d)", scanner_lat, scanner_lon, avg_rssi, rssi_count); } else { snprintf(msg, sizeof(msg), "MLAT:L;%.2f;%.2f;%d", scanner_x, scanner_y, avg_rssi); ESP_LOGD(TAG, "sent: local=(%.2f,%.2f)m rssi=%d (avg of %d)", scanner_x, scanner_y, avg_rssi, rssi_count); } msg_info(TAG, msg, NULL); } } send_task_handle = NULL; ESP_LOGI(TAG, "send task stopped"); vTaskDelete(NULL); } /* ============================================================ * BLE INIT / DEINIT * ============================================================ */ static bool ble_init(void) { if (ble_initialized) { return true; } esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); esp_err_t ret = esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT); if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) { ESP_LOGE(TAG, "bt mem release failed: %s", esp_err_to_name(ret)); return false; } ret = esp_bt_controller_init(&bt_cfg); if (ret != ESP_OK) { ESP_LOGE(TAG, "bt controller init failed: %s", esp_err_to_name(ret)); return false; } ret = esp_bt_controller_enable(ESP_BT_MODE_BLE); if (ret != ESP_OK) { ESP_LOGE(TAG, "bt controller enable failed: %s", esp_err_to_name(ret)); return false; } ret = esp_bluedroid_init(); if (ret != ESP_OK) { ESP_LOGE(TAG, "bluedroid init failed: %s", esp_err_to_name(ret)); return false; } ret = esp_bluedroid_enable(); if (ret != ESP_OK) { ESP_LOGE(TAG, "bluedroid enable failed: %s", esp_err_to_name(ret)); return false; } ret = esp_ble_gap_register_callback(ble_scan_cb); if (ret != ESP_OK) { ESP_LOGE(TAG, "gap register callback failed: %s", esp_err_to_name(ret)); return false; } esp_ble_scan_params_t scan_params = { .scan_type = BLE_SCAN_TYPE_ACTIVE, .own_addr_type = BLE_ADDR_TYPE_PUBLIC, .scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL, .scan_interval = 0x50, .scan_window = 0x30, .scan_duplicate = BLE_SCAN_DUPLICATE_DISABLE }; ret = esp_ble_gap_set_scan_params(&scan_params); if (ret != ESP_OK) { ESP_LOGE(TAG, "set scan params failed: %s", esp_err_to_name(ret)); return false; } ble_initialized = true; ESP_LOGI(TAG, "BLE initialized"); return true; } static bool ble_start_scan(void) { esp_err_t ret = esp_ble_gap_start_scanning(0); /* 0 = continuous */ if (ret != ESP_OK) { ESP_LOGE(TAG, "start BLE scanning failed: %s", esp_err_to_name(ret)); return false; } return true; } static void ble_stop_scan(void) { esp_ble_gap_stop_scanning(); } /* ============================================================ * WIFI PROMISCUOUS INIT / DEINIT * ============================================================ */ static bool wifi_promisc_init(void) { if (wifi_promisc_enabled) { return true; } /* Enable promiscuous mode */ esp_err_t ret = esp_wifi_set_promiscuous(true); if (ret != ESP_OK) { ESP_LOGE(TAG, "set promiscuous failed: %s", esp_err_to_name(ret)); return false; } /* Register callback */ ret = esp_wifi_set_promiscuous_rx_cb(wifi_promisc_cb); if (ret != ESP_OK) { ESP_LOGE(TAG, "set promiscuous cb failed: %s", esp_err_to_name(ret)); esp_wifi_set_promiscuous(false); return false; } /* Filter only management frames */ wifi_promiscuous_filter_t filter = { .filter_mask = WIFI_PROMIS_FILTER_MASK_MGMT }; esp_wifi_set_promiscuous_filter(&filter); wifi_promisc_enabled = true; ESP_LOGI(TAG, "WiFi promiscuous mode enabled"); return true; } static void wifi_promisc_deinit(void) { if (!wifi_promisc_enabled) return; esp_wifi_set_promiscuous(false); wifi_promisc_enabled = false; ESP_LOGI(TAG, "WiFi promiscuous mode disabled"); } /* ============================================================ * START / STOP SCANNING * ============================================================ */ static bool start_scanning(void) { reset_rssi_history(); if (mlat_mode == MLAT_MODE_BLE) { if (!ble_init()) return false; if (!ble_start_scan()) return false; } else if (mlat_mode == MLAT_MODE_WIFI) { if (!wifi_promisc_init()) return false; /* Start channel hop task for WiFi */ BaseType_t ret = xTaskCreate( channel_hop_task, "mlat_hop", 2048, NULL, 4, &hop_task_handle ); if (ret != pdPASS) { ESP_LOGE(TAG, "failed to create hop task"); wifi_promisc_deinit(); return false; } } /* Start send task */ BaseType_t ret = xTaskCreate( mlat_send_task, "mlat_send", 4096, NULL, 5, &send_task_handle ); if (ret != pdPASS) { ESP_LOGE(TAG, "failed to create send task"); if (mlat_mode == MLAT_MODE_BLE) { ble_stop_scan(); } else { wifi_promisc_deinit(); } return false; } return true; } static void stop_scanning(void) { if (mlat_mode == MLAT_MODE_BLE) { ble_stop_scan(); } else if (mlat_mode == MLAT_MODE_WIFI) { wifi_promisc_deinit(); } } /* ============================================================ * COMMAND: mlat config * mlat config gps - GPS coordinates (degrees) * mlat config local - Local coordinates (meters) * mlat config - Backward compat: GPS mode * ============================================================ */ static int cmd_mlat_config(int argc, char **argv, const char *req, void *ctx) { (void)ctx; if (argc < 2) { msg_error(TAG, "usage: mlat config [gps|local] ", req); return -1; } char msg[100]; /* Check if first arg is coordinate type */ if (argc == 3 && strcasecmp(argv[0], "gps") == 0) { /* GPS mode: mlat config gps */ double lat = strtod(argv[1], NULL); double lon = strtod(argv[2], NULL); if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) { msg_error(TAG, "invalid GPS coords (lat:-90~90, lon:-180~180)", req); return -1; } coord_type = COORD_GPS; scanner_lat = lat; scanner_lon = lon; mlat_configured = true; snprintf(msg, sizeof(msg), "GPS position: (%.6f, %.6f)", lat, lon); msg_info(TAG, msg, req); ESP_LOGI(TAG, "configured GPS: lat=%.6f lon=%.6f", scanner_lat, scanner_lon); } else if (argc == 3 && strcasecmp(argv[0], "local") == 0) { /* Local mode: mlat config local */ double x = strtod(argv[1], NULL); double y = strtod(argv[2], NULL); coord_type = COORD_LOCAL; scanner_x = x; scanner_y = y; mlat_configured = true; snprintf(msg, sizeof(msg), "Local position: (%.2f, %.2f) meters", x, y); msg_info(TAG, msg, req); ESP_LOGI(TAG, "configured local: x=%.2f y=%.2f", scanner_x, scanner_y); } else if (argc == 2) { /* Backward compat: mlat config -> GPS mode */ double lat = strtod(argv[0], NULL); double lon = strtod(argv[1], NULL); if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) { msg_error(TAG, "invalid GPS coords (lat:-90~90, lon:-180~180)", req); return -1; } coord_type = COORD_GPS; scanner_lat = lat; scanner_lon = lon; mlat_configured = true; snprintf(msg, sizeof(msg), "GPS position: (%.6f, %.6f)", lat, lon); msg_info(TAG, msg, req); ESP_LOGI(TAG, "configured GPS: lat=%.6f lon=%.6f", scanner_lat, scanner_lon); } else { msg_error(TAG, "usage: mlat config [gps|local] ", req); return -1; } return 0; } /* ============================================================ * COMMAND: mlat mode * ============================================================ */ static int cmd_mlat_mode(int argc, char **argv, const char *req, void *ctx) { (void)ctx; if (argc != 1) { msg_error(TAG, "usage: mlat mode ", req); return -1; } if (mlat_running) { msg_error(TAG, "stop scanning first", req); return -1; } const char *mode_str = argv[0]; if (strcasecmp(mode_str, "ble") == 0) { mlat_mode = MLAT_MODE_BLE; } else if (strcasecmp(mode_str, "wifi") == 0) { mlat_mode = MLAT_MODE_WIFI; } else { msg_error(TAG, "invalid mode (use: ble, wifi)", req); return -1; } char msg[32]; snprintf(msg, sizeof(msg), "mode set to %s", mode_to_str(mlat_mode)); msg_info(TAG, msg, req); ESP_LOGI(TAG, "mode changed to %s", mode_to_str(mlat_mode)); return 0; } /* ============================================================ * COMMAND: mlat start * ============================================================ */ static int cmd_mlat_start(int argc, char **argv, const char *req, void *ctx) { (void)ctx; if (argc != 1) { msg_error(TAG, "usage: mlat start ", req); return -1; } if (mlat_running) { msg_error(TAG, "already running", req); return -1; } if (!mlat_configured) { msg_error(TAG, "not configured - run 'mlat config [gps|local] ' first", req); return -1; } /* Parse target MAC */ if (!parse_mac_str(argv[0], target_mac)) { msg_error(TAG, "invalid MAC address", req); return -1; } mac_to_str(target_mac, target_mac_str, sizeof(target_mac_str)); mlat_running = true; if (!start_scanning()) { mlat_running = false; msg_error(TAG, "scan start failed", req); return -1; } char msg[128]; if (coord_type == COORD_GPS) { snprintf(msg, sizeof(msg), "scanning for %s at GPS(%.6f, %.6f) [%s]", target_mac_str, scanner_lat, scanner_lon, mode_to_str(mlat_mode)); ESP_LOGI(TAG, "started: target=%s GPS=(%.6f,%.6f) mode=%s", target_mac_str, scanner_lat, scanner_lon, mode_to_str(mlat_mode)); } else { snprintf(msg, sizeof(msg), "scanning for %s at local(%.2f, %.2f)m [%s]", target_mac_str, scanner_x, scanner_y, mode_to_str(mlat_mode)); ESP_LOGI(TAG, "started: target=%s local=(%.2f,%.2f)m mode=%s", target_mac_str, scanner_x, scanner_y, mode_to_str(mlat_mode)); } msg_info(TAG, msg, req); return 0; } /* ============================================================ * COMMAND: mlat stop * ============================================================ */ static int cmd_mlat_stop(int argc, char **argv, const char *req, void *ctx) { (void)argc; (void)argv; (void)ctx; if (!mlat_running) { msg_error(TAG, "not running", req); return -1; } mlat_running = false; stop_scanning(); msg_info(TAG, "stopped", req); ESP_LOGI(TAG, "stopped"); return 0; } /* ============================================================ * COMMAND: mlat status * ============================================================ */ static int cmd_mlat_status(int argc, char **argv, const char *req, void *ctx) { (void)argc; (void)argv; (void)ctx; char msg[180]; const char *coord_str = (coord_type == COORD_GPS) ? "GPS" : "Local"; if (!mlat_configured) { snprintf(msg, sizeof(msg), "not configured | mode=%s", mode_to_str(mlat_mode)); msg_info(TAG, msg, req); return 0; } /* Format position based on coord type */ char pos_str[60]; if (coord_type == COORD_GPS) { snprintf(pos_str, sizeof(pos_str), "GPS=(%.6f,%.6f)", scanner_lat, scanner_lon); } else { snprintf(pos_str, sizeof(pos_str), "local=(%.2f,%.2f)m", scanner_x, scanner_y); } if (mlat_running) { int8_t avg = get_average_rssi(); if (mlat_mode == MLAT_MODE_WIFI) { snprintf(msg, sizeof(msg), "running [%s] | %s | target=%s | rssi=%d (%d) | ch=%d", mode_to_str(mlat_mode), pos_str, target_mac_str, avg, rssi_count, current_channel); } else { snprintf(msg, sizeof(msg), "running [%s] | %s | target=%s | rssi=%d (%d samples)", mode_to_str(mlat_mode), pos_str, target_mac_str, avg, rssi_count); } } else { snprintf(msg, sizeof(msg), "stopped | mode=%s | %s", mode_to_str(mlat_mode), pos_str); } msg_info(TAG, msg, req); return 0; } /* ============================================================ * COMMAND DEFINITIONS * ============================================================ */ static const command_t cmd_mlat_config_def = { .name = "mlat", .sub = "config", .help = "Set position: mlat config [gps|local] ", .handler = cmd_mlat_config, .ctx = NULL, .async = false, .min_args = 2, .max_args = 3 }; static const command_t cmd_mlat_mode_def = { .name = "mlat", .sub = "mode", .help = "Set scan mode: mlat mode ", .handler = cmd_mlat_mode, .ctx = NULL, .async = false, .min_args = 1, .max_args = 1 }; static const command_t cmd_mlat_start_def = { .name = "mlat", .sub = "start", .help = "Start scanning: mlat start ", .handler = cmd_mlat_start, .ctx = NULL, .async = false, .min_args = 1, .max_args = 1 }; static const command_t cmd_mlat_stop_def = { .name = "mlat", .sub = "stop", .help = "Stop scanning", .handler = cmd_mlat_stop, .ctx = NULL, .async = false, .min_args = 0, .max_args = 0 }; static const command_t cmd_mlat_status_def = { .name = "mlat", .sub = "status", .help = "Show MLAT status", .handler = cmd_mlat_status, .ctx = NULL, .async = false, .min_args = 0, .max_args = 0 }; /* ============================================================ * REGISTER * ============================================================ */ void mod_mlat_register_commands(void) { command_register(&cmd_mlat_config_def); command_register(&cmd_mlat_mode_def); command_register(&cmd_mlat_start_def); command_register(&cmd_mlat_stop_def); command_register(&cmd_mlat_status_def); ESP_LOGI(TAG, "commands registered (BLE+WiFi)"); } #endif /* CONFIG_RECON_MODE_MLAT */