diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index e9647443a9bbf0ea429fa31c8d0be004c7a436b6..f8ca4994c50192342e40715f62d50805b629ba14 100755
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,3 +1,3 @@
-idf_component_register(SRCS "ftm_main.c"
+idf_component_register(SRCS "math_3d.c" "ftm.c" "commands.c" "ftm_main.c"
INCLUDE_DIRS ".")
target_compile_options(${COMPONENT_LIB} PRIVATE "-Wno-format")
diff --git a/main/commands.c b/main/commands.c
new file mode 100644
index 0000000000000000000000000000000000000000..659ce4b8cdd992040c30eb9f0983c9c77bd6ad9e
--- /dev/null
+++ b/main/commands.c
@@ -0,0 +1,369 @@
+#include "commands.h"
+
+#include <stdio.h>
+
+
+wifi_args_t sta_args;
+wifi_args_t sta_args;
+wifi_args_t ap_args;
+wifi_scan_arg_t scan_args;
+wifi_ftm_args_t ftm_args;
+wifi_calibrate_args_t calibrate_args;
+
+int wifi_cmd_sta(int argc, char **argv) {
+ int nerrors = arg_parse(argc, argv, (void **)&sta_args);
+
+ if (nerrors != 0) {
+ arg_print_errors(stderr, sta_args.end, argv[0]);
+ return 1;
+ }
+
+ ESP_LOGI(TAG_STA, "sta connecting to '%s'", sta_args.ssid->sval[0]);
+ wifi_cmd_sta_join(sta_args.ssid->sval[0], sta_args.password->sval[0]);
+ return 0;
+}
+
+bool wifi_cmd_sta_join(const char *ssid, const char *pass) {
+ int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+
+ wifi_config_t wifi_config = {0};
+
+ strlcpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
+ if (pass) {
+ strlcpy((char *)wifi_config.sta.password, pass,
+ sizeof(wifi_config.sta.password));
+ }
+
+ if (bits & CONNECTED_BIT) {
+ s_reconnect = false;
+ xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
+ ESP_ERROR_CHECK(esp_wifi_disconnect());
+ xEventGroupWaitBits(s_wifi_event_group, DISCONNECTED_BIT, 0, 1,
+ portTICK_PERIOD_MS);
+ }
+
+ s_reconnect = true;
+ s_retry_num = 0;
+ ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+ ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
+ ESP_ERROR_CHECK(esp_wifi_connect());
+
+ xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1,
+ 5000 / portTICK_PERIOD_MS);
+
+ return true;
+}
+
+int wifi_cmd_scan(int argc, char **argv) {
+ int nerrors = arg_parse(argc, argv, (void **)&scan_args);
+
+ if (nerrors != 0) {
+ arg_print_errors(stderr, scan_args.end, argv[0]);
+ return 1;
+ }
+
+ ESP_LOGI(TAG_STA, "sta start to scan");
+ if (scan_args.ssid->count == 1) {
+ wifi_perform_scan(scan_args.ssid->sval[0], false);
+ } else {
+ wifi_perform_scan(NULL, false);
+ }
+ return 0;
+}
+
+bool wifi_cmd_ap_set(const char *ssid, const char *pass) {
+ s_reconnect = false;
+ strlcpy((char *)g_ap_config.ap.ssid, ssid, MAX_SSID_LEN);
+ if (pass) {
+ if (strlen(pass) != 0 && strlen(pass) < 8) {
+ s_reconnect = true;
+ ESP_LOGE(TAG_AP, "password cannot be less than 8 characters long");
+ return false;
+ }
+ strlcpy((char *)g_ap_config.ap.password, pass, MAX_PASSPHRASE_LEN);
+ }
+
+ if (strlen(pass) == 0) {
+ g_ap_config.ap.authmode = WIFI_AUTH_OPEN;
+ }
+
+ ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
+ ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+ return true;
+}
+
+int wifi_cmd_ap(int argc, char **argv) {
+ int nerrors = arg_parse(argc, argv, (void **)&ap_args);
+
+ if (nerrors != 0) {
+ arg_print_errors(stderr, ap_args.end, argv[0]);
+ return 1;
+ }
+
+ if (true == wifi_cmd_ap_set(ap_args.ssid->sval[0], ap_args.password->sval[0]))
+ ESP_LOGI(
+ TAG_AP,
+ "Starting SoftAP with FTM Responder support, SSID - %s, Password - %s",
+ ap_args.ssid->sval[0], ap_args.password->sval[0]);
+ else
+ ESP_LOGE(TAG_AP, "Failed to start SoftAP!");
+
+ return 0;
+}
+
+int wifi_cmd_query(int argc, char **argv) {
+ wifi_config_t cfg;
+ wifi_mode_t mode;
+
+ esp_wifi_get_mode(&mode);
+ if (WIFI_MODE_AP == mode) {
+ esp_wifi_get_config(WIFI_IF_AP, &cfg);
+ ESP_LOGI(TAG_AP, "AP mode, %s %s", cfg.ap.ssid, cfg.ap.password);
+ } else if (WIFI_MODE_STA == mode) {
+ int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+ if (bits & CONNECTED_BIT) {
+ esp_wifi_get_config(WIFI_IF_STA, &cfg);
+ ESP_LOGI(TAG_STA, "sta mode, connected %s", cfg.ap.ssid);
+ } else {
+ ESP_LOGI(TAG_STA, "sta mode, disconnected");
+ }
+ } else {
+ ESP_LOGI(TAG_STA, "NULL mode");
+ return 0;
+ }
+
+ return 0;
+}
+
+int wifi_cmd_list_offsets(int argc, char **argv) {
+ ESP_LOGI(TAG_STA, "-------- OFFSETS --------");
+ for (size_t i = 0; i < 4; i++) {
+ ESP_LOGI(TAG_STA, "AP_%d : %.2f", i, calibration_offsets[i]);
+ }
+
+ return 0;
+}
+
+int wifi_cmd_ftm(int argc, char **argv) {
+ int nerrors = arg_parse(argc, argv, (void **)&ftm_args);
+ wifi_ap_record_t *ap_record;
+ EventBits_t bits;
+
+ wifi_ftm_initiator_cfg_t ftmi_cfg = {
+ .frm_count = 32,
+ .burst_period = 2,
+ };
+
+ if (nerrors != 0) {
+ arg_print_errors(stderr, ftm_args.end, argv[0]);
+ return 0;
+ }
+
+ if (ftm_args.initiator->count != 0 && ftm_args.responder->count != 0) {
+ ESP_LOGE(TAG_STA, "Invalid FTM cmd argument");
+ return 0;
+ }
+
+ if (ftm_args.responder->count != 0)
+ goto ftm_responder;
+
+ bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+ if (bits & CONNECTED_BIT && !ftm_args.ssid->count) {
+ memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
+ ftmi_cfg.channel = s_ap_channel;
+ } else if (ftm_args.ssid->count == 1) {
+ ap_record = find_ftm_responder_ap(ftm_args.ssid->sval[0]);
+ if (ap_record) {
+ memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
+ ftmi_cfg.channel = ap_record->primary;
+ } else {
+ return 0;
+ }
+ } else {
+ ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
+ return 0;
+ }
+
+ if (ftm_args.frm_count->count != 0) {
+ uint8_t count = ftm_args.frm_count->ival[0];
+ if (count != 0 && count != 8 && count != 16 && count != 24 && count != 32 &&
+ count != 64) {
+ ESP_LOGE(TAG_STA,
+ "Invalid Frame Count! Valid options are 0/8/16/24/32/64");
+ return 0;
+ }
+ ftmi_cfg.frm_count = count;
+ }
+
+ if (ftm_args.burst_period->count != 0) {
+ if (ftm_args.burst_period->ival[0] >= 2 &&
+ ftm_args.burst_period->ival[0] < 256) {
+ ftmi_cfg.burst_period = ftm_args.burst_period->ival[0];
+ } else {
+ ESP_LOGE(TAG_STA, "Invalid Burst Period! Valid range is 2-255");
+ return 0;
+ }
+ }
+
+ ESP_LOGI(TAG_STA,
+ "Requesting FTM session with Frm Count - %d, Burst Period - %dmSec "
+ "(0: No Preference)",
+ ftmi_cfg.frm_count, ftmi_cfg.burst_period * 100);
+
+ if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
+ ESP_LOGE(TAG_STA, "Failed to start FTM session");
+ return 0;
+ }
+
+ bits =
+ xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
+ pdTRUE, pdFALSE, portMAX_DELAY);
+ /* Processing data from FTM session */
+ if (bits & FTM_REPORT_BIT) {
+ ftm_process_report();
+ free(s_ftm_report);
+ s_ftm_report = NULL;
+ s_ftm_report_num_entries = 0;
+ ESP_LOGI(TAG_STA,
+ "Estimated RTT - %d nSec, Estimated Distance - %d.%02d meters",
+ s_rtt_est, s_dist_est / 100, s_dist_est % 100);
+ } else {
+ /* Failure case */
+ }
+
+ return 0;
+
+ftm_responder:
+ if (ftm_args.offset->count != 0) {
+ int16_t offset_cm = ftm_args.offset->ival[0];
+
+ esp_wifi_ftm_resp_set_offset(offset_cm);
+ }
+
+ if (ftm_args.enable->count != 0) {
+ if (!s_ap_started) {
+ ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
+ return 0;
+ }
+ g_ap_config.ap.ftm_responder = true;
+ ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+ ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder enabled");
+
+ return 0;
+ }
+
+ if (ftm_args.disable->count != 0) {
+ if (!s_ap_started) {
+ ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
+ return 0;
+ }
+ g_ap_config.ap.ftm_responder = false;
+ ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
+ ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder disabled");
+ }
+
+ return 0;
+}
+
+/* Calibration Routine */
+int wifi_cmd_calibrate(int argc, char **argv) {
+ int nerrors = arg_parse(argc, argv, (void **)&calibrate_args);
+ wifi_ap_record_t *ap_record;
+ EventBits_t bits;
+
+ wifi_ftm_initiator_cfg_t ftmi_cfg = {
+ .frm_count = 8,
+ .burst_period = 2,
+ };
+
+ if (nerrors != 0) {
+ arg_print_errors(stderr, calibrate_args.end, argv[0]);
+ return 0;
+ }
+
+ bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
+ if (bits & CONNECTED_BIT && !calibrate_args.ssid->count) {
+ memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
+ ftmi_cfg.channel = s_ap_channel;
+ } else if (calibrate_args.ssid->count == 1) {
+ ap_record = find_ftm_responder_ap(calibrate_args.ssid->sval[0]);
+ if (ap_record) {
+ memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
+ ftmi_cfg.channel = ap_record->primary;
+ } else {
+ return 0;
+ }
+ } else {
+ ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
+ return 0;
+ }
+
+ /**
+ * Check if actual distance is set
+ */
+
+ if (calibrate_args.actual_distance->count != 0) {
+ if (calibrate_args.actual_distance->dval[0] < 0) {
+ ESP_LOGE(TAG_STA, "Invalid distance, please enter a positive value");
+ return 0;
+ }
+ }
+
+ /**
+ * Check if IP id is correct
+ */
+
+ if (calibrate_args.ap_id->count != 0) {
+ uint8_t ap_id = calibrate_args.ap_id->ival[0];
+ if (ap_id != 0 && ap_id != 1 && ap_id != 2 && ap_id != 3) {
+ ESP_LOGE(TAG_STA, "Invalid AP id. Valid options are 0/1/2/3");
+ return 0;
+ }
+ } else {
+ ESP_LOGE(TAG_STA,
+ "Please provide an AP id to calibrate. Valid options are 0/1/2/3");
+ return 0;
+ }
+
+ ESP_LOGI(TAG_STA,
+ "Calibration Routine with %s\nRequesting FTM session with Frm Count "
+ "- %d, Burst Period - %dmSec (0: No Preference)",
+ calibrate_args.ssid->sval[0], ftmi_cfg.frm_count,
+ ftmi_cfg.burst_period * 100);
+
+ if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
+ ESP_LOGE(TAG_STA, "Failed to start FTM session");
+ return 0;
+ }
+
+ bits =
+ xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
+ pdTRUE, pdFALSE, portMAX_DELAY);
+ /* Processing data from FTM session */
+ if (bits & FTM_REPORT_BIT) {
+ ftm_process_report();
+ free(s_ftm_report);
+ s_ftm_report = NULL;
+ s_ftm_report_num_entries = 0;
+
+ ESP_LOGI(TAG_STA, "-------- CALIBRATION --------");
+
+ calibration_offsets[calibrate_args.ap_id->ival[0]] =
+ (calibrate_args.actual_distance->dval[0] * 100 - s_dist_est) / 100;
+
+ ESP_LOGI(TAG_STA, "Calibrating AP_%d", calibrate_args.ap_id->ival[0]);
+ ESP_LOGI(TAG_STA, "Estimated RTT : %d nSec\n", s_rtt_est);
+
+ ESP_LOGI(TAG_STA, "Estimated distance (no calibration) : %d.%02d",
+ s_dist_est / 100, s_dist_est % 100);
+ ESP_LOGI(TAG_STA, "Actual distance : %.2f",
+ calibrate_args.actual_distance->dval[0]);
+ ESP_LOGI(TAG_STA, "Calibration offset : %.2f",
+ calibration_offsets[calibrate_args.ap_id->ival[0]]);
+
+ } else {
+ /* Failure case */
+ }
+
+ return 0;
+}
\ No newline at end of file
diff --git a/main/commands.h b/main/commands.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c535e67f10f93db4ce7f4cf9274967e8dd420b3
--- /dev/null
+++ b/main/commands.h
@@ -0,0 +1,84 @@
+#ifndef COMMANDS_INCLUDE
+#define COMMANDS_INCLUDE
+
+#include "argtable3/argtable3.h"
+#include "cmd_system.h"
+#include "esp_console.h"
+#include "esp_err.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "esp_mac.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "nvs_flash.h"
+
+#include "ftm.h"
+
+
+/*
+ * Arguments for Wifi related commands
+ */
+typedef struct {
+ struct arg_str *ssid;
+ struct arg_str *password;
+ struct arg_end *end;
+} wifi_args_t;
+
+
+/*
+ * Arguments for the Wifi scan command
+ */
+typedef struct {
+ struct arg_str *ssid;
+ struct arg_end *end;
+} wifi_scan_arg_t;
+
+
+typedef struct {
+ /* FTM Initiator */
+ struct arg_lit *initiator;
+ struct arg_int *frm_count;
+ struct arg_int *burst_period;
+ struct arg_str *ssid;
+ /* FTM Responder */
+ struct arg_lit *responder;
+ struct arg_lit *enable;
+ struct arg_lit *disable;
+ struct arg_int *offset;
+ struct arg_end *end;
+} wifi_ftm_args_t;
+
+typedef struct {
+ struct arg_dbl *actual_distance;
+ struct arg_str *ssid;
+ struct arg_int *ap_id;
+ struct arg_end *end;
+} wifi_calibrate_args_t;
+
+extern wifi_args_t sta_args;
+extern wifi_args_t sta_args;
+extern wifi_args_t ap_args;
+extern wifi_scan_arg_t scan_args;
+extern wifi_ftm_args_t ftm_args;
+extern wifi_calibrate_args_t calibrate_args;
+
+int wifi_cmd_sta(int argc, char **argv);
+
+bool wifi_cmd_sta_join(const char *ssid, const char *pass);
+
+int wifi_cmd_scan(int argc, char **argv);
+
+bool wifi_cmd_ap_set(const char *ssid, const char *pass);
+
+int wifi_cmd_ap(int argc, char **argv);
+
+int wifi_cmd_query(int argc, char **argv);
+
+int wifi_cmd_list_offsets(int argc, char **argv);
+
+int wifi_cmd_ftm(int argc, char **argv);
+
+int wifi_cmd_calibrate(int argc, char **argv);
+
+#endif //include guard
\ No newline at end of file
diff --git a/main/ftm.c b/main/ftm.c
new file mode 100644
index 0000000000000000000000000000000000000000..afe79da60d412c8e85001d7a6a4369705a57b8d6
--- /dev/null
+++ b/main/ftm.c
@@ -0,0 +1,169 @@
+#include "ftm.h"
+
+
+const char *TAG_STA = "ftm_station";
+const char *TAG_AP = "ftm_ap";
+
+wifi_config_t g_ap_config = {.ap.max_connection = 4,
+ .ap.authmode = WIFI_AUTH_WPA2_PSK,
+ .ap.ftm_responder = true};
+
+double calibration_offsets[4];
+
+bool s_reconnect = true;
+int s_retry_num = 0;
+
+EventGroupHandle_t s_wifi_event_group;
+const int CONNECTED_BIT = BIT0;
+const int DISCONNECTED_BIT = BIT1;
+
+EventGroupHandle_t s_ftm_event_group;
+const int FTM_REPORT_BIT = BIT0;
+int FTM_FAILURE_BIT = BIT1;
+wifi_ftm_report_entry_t *s_ftm_report;
+uint8_t s_ftm_report_num_entries;
+uint32_t s_rtt_est, s_dist_est;
+bool s_ap_started;
+uint8_t s_ap_channel;
+uint8_t s_ap_bssid[ETH_ALEN];
+
+const int g_report_lvl =
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_DIAG
+ BIT0 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RTT
+ BIT1 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_T1T2T3T4
+ BIT2 |
+#endif
+#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RSSI
+ BIT3 |
+#endif
+ 0;
+
+uint16_t g_scan_ap_num;
+wifi_ap_record_t *g_ap_list_buffer;
+
+void ftm_process_report(void) {
+ int i;
+ char *log = NULL;
+
+ if (!g_report_lvl)
+ return;
+
+ log = malloc(200);
+ if (!log) {
+ ESP_LOGE(TAG_STA, "Failed to alloc buffer for FTM report");
+ return;
+ }
+
+ bzero(log, 200);
+ sprintf(log, "%s%s%s%s", g_report_lvl & BIT0 ? " Diag |" : "",
+ g_report_lvl & BIT1 ? " RTT |" : "",
+ g_report_lvl & BIT2 ? " T1 | T2 | T3 "
+ " | T4 |"
+ : "",
+ g_report_lvl & BIT3 ? " RSSI |" : "");
+ ESP_LOGI(TAG_STA, "FTM Report:");
+ ESP_LOGI(TAG_STA, "|%s", log);
+ for (i = 0; i < s_ftm_report_num_entries; i++) {
+ char *log_ptr = log;
+
+ bzero(log, 200);
+ if (g_report_lvl & BIT0) {
+ log_ptr += sprintf(log_ptr, "%6d|", s_ftm_report[i].dlog_token);
+ }
+ if (g_report_lvl & BIT1) {
+ log_ptr += sprintf(log_ptr, "%7u |", s_ftm_report[i].rtt);
+ }
+ if (g_report_lvl & BIT2) {
+ log_ptr += sprintf(log_ptr, "%14llu |%14llu |%14llu |%14llu |",
+ s_ftm_report[i].t1, s_ftm_report[i].t2,
+ s_ftm_report[i].t3, s_ftm_report[i].t4);
+ }
+ if (g_report_lvl & BIT3) {
+ log_ptr += sprintf(log_ptr, "%6d |", s_ftm_report[i].rssi);
+ }
+ ESP_LOGI(TAG_STA, "|%s", log);
+ }
+ free(log);
+}
+
+bool wifi_perform_scan(const char *ssid, bool internal) {
+ wifi_scan_config_t scan_config = {0};
+ scan_config.ssid = (uint8_t *)ssid;
+ uint8_t i;
+
+ ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+ if (ESP_OK != esp_wifi_scan_start(&scan_config, true)) {
+ ESP_LOGI(TAG_STA, "Failed to perform scan");
+ return false;
+ }
+
+ esp_wifi_scan_get_ap_num(&g_scan_ap_num);
+ if (g_scan_ap_num == 0) {
+ ESP_LOGI(TAG_STA, "No matching AP found");
+ return false;
+ }
+
+ if (g_ap_list_buffer) {
+ free(g_ap_list_buffer);
+ }
+ g_ap_list_buffer = malloc(g_scan_ap_num * sizeof(wifi_ap_record_t));
+ if (g_ap_list_buffer == NULL) {
+ ESP_LOGE(TAG_STA, "Failed to malloc buffer to print scan results");
+ return false;
+ }
+
+ if (esp_wifi_scan_get_ap_records(
+ &g_scan_ap_num, (wifi_ap_record_t *)g_ap_list_buffer) == ESP_OK) {
+ if (!internal) {
+ for (i = 0; i < g_scan_ap_num; i++) {
+ ESP_LOGI(TAG_STA,
+ "[%s][rssi=%d]"
+ "%s",
+ g_ap_list_buffer[i].ssid, g_ap_list_buffer[i].rssi,
+ g_ap_list_buffer[i].ftm_responder ? "[FTM Responder]" : "");
+ }
+ }
+ }
+
+ ESP_LOGI(TAG_STA, "sta scan done");
+
+ return true;
+}
+
+wifi_ap_record_t *find_ftm_responder_ap(const char *ssid) {
+ bool retry_scan = false;
+ uint8_t i;
+
+ if (!ssid)
+ return NULL;
+
+retry:
+ if (!g_ap_list_buffer || (g_scan_ap_num == 0)) {
+ ESP_LOGI(TAG_STA, "Scanning for %s", ssid);
+ if (false == wifi_perform_scan(ssid, true)) {
+ return NULL;
+ }
+ }
+
+ for (i = 0; i < g_scan_ap_num; i++) {
+ if (strcmp((const char *)g_ap_list_buffer[i].ssid, ssid) == 0)
+ return &g_ap_list_buffer[i];
+ }
+
+ if (!retry_scan) {
+ retry_scan = true;
+ if (g_ap_list_buffer) {
+ free(g_ap_list_buffer);
+ g_ap_list_buffer = NULL;
+ }
+ goto retry;
+ }
+
+ ESP_LOGI(TAG_STA, "No matching AP found");
+
+ return NULL;
+}
\ No newline at end of file
diff --git a/main/ftm.h b/main/ftm.h
new file mode 100644
index 0000000000000000000000000000000000000000..58de7e0e3ed77ed9f208d1adcd031fa96b05781e
--- /dev/null
+++ b/main/ftm.h
@@ -0,0 +1,56 @@
+#ifndef FTM_INCLUDE
+#define FTM_INCLUDE
+
+#include "argtable3/argtable3.h"
+#include "cmd_system.h"
+#include "esp_console.h"
+#include "esp_err.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "esp_mac.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "nvs_flash.h"
+#include <string.h>
+
+
+extern const char *TAG_STA;
+extern const char *TAG_AP;
+
+// Calibration offsets array
+extern double calibration_offsets[4];
+
+extern wifi_config_t g_ap_config;
+
+#define ETH_ALEN 6
+#define MAX_CONNECT_RETRY_ATTEMPTS 5
+
+extern bool s_reconnect;
+extern int s_retry_num;
+
+extern EventGroupHandle_t s_wifi_event_group;
+extern const int CONNECTED_BIT;
+extern const int DISCONNECTED_BIT;
+extern EventGroupHandle_t s_ftm_event_group;
+extern const int FTM_REPORT_BIT;
+extern int FTM_FAILURE_BIT;
+extern wifi_ftm_report_entry_t *s_ftm_report;
+extern uint8_t s_ftm_report_num_entries;
+extern uint32_t s_rtt_est, s_dist_est;
+extern bool s_ap_started;
+extern uint8_t s_ap_channel;
+extern uint8_t s_ap_bssid[ETH_ALEN];
+
+extern const int g_report_lvl;
+
+extern uint16_t g_scan_ap_num;
+extern wifi_ap_record_t *g_ap_list_buffer;
+
+bool wifi_perform_scan(const char *ssid, bool internal);
+
+wifi_ap_record_t *find_ftm_responder_ap(const char *ssid);
+
+void ftm_process_report(void);
+
+#endif // include guard
\ No newline at end of file
diff --git a/main/ftm_main.c b/main/ftm_main.c
index cb913b522999e09afaa7669904fa25e587becde1..cbdec16b4080eebb472abc5ba633cb362fd926cd 100755
--- a/main/ftm_main.c
+++ b/main/ftm_main.c
@@ -23,159 +23,9 @@
#include <stdio.h>
#include <string.h>
-typedef struct {
- struct arg_str *ssid;
- struct arg_str *password;
- struct arg_end *end;
-} wifi_args_t;
-
-typedef struct {
- struct arg_str *ssid;
- struct arg_end *end;
-} wifi_scan_arg_t;
-
-typedef struct {
- /* FTM Initiator */
- struct arg_lit *initiator;
- struct arg_int *frm_count;
- struct arg_int *burst_period;
- struct arg_str *ssid;
- /* FTM Responder */
- struct arg_lit *responder;
- struct arg_lit *enable;
- struct arg_lit *disable;
- struct arg_int *offset;
- struct arg_end *end;
-} wifi_ftm_args_t;
-
-typedef struct {
- struct arg_dbl *actual_distance;
- struct arg_str *ssid;
- struct arg_int *ap_id;
- struct arg_end *end;
-} wifi_calibrate_args_t;
-
-// Point structure
-typedef struct {
- double x;
- double y;
- double z;
-} point_t;
-
-static wifi_args_t sta_args;
-static wifi_args_t ap_args;
-static wifi_scan_arg_t scan_args;
-static wifi_ftm_args_t ftm_args;
-static wifi_calibrate_args_t calibrate_args;
-
-// Calibration offsets array
-double calibration_offsets[4];
-
-wifi_config_t g_ap_config = {.ap.max_connection = 4,
- .ap.authmode = WIFI_AUTH_WPA2_PSK,
- .ap.ftm_responder = true};
-
-#define ETH_ALEN 6
-#define MAX_CONNECT_RETRY_ATTEMPTS 5
-
-static bool s_reconnect = true;
-static int s_retry_num = 0;
-static const char *TAG_STA = "ftm_station";
-static const char *TAG_AP = "ftm_ap";
-
-static EventGroupHandle_t s_wifi_event_group;
-static const int CONNECTED_BIT = BIT0;
-static const int DISCONNECTED_BIT = BIT1;
-
-static EventGroupHandle_t s_ftm_event_group;
-static const int FTM_REPORT_BIT = BIT0;
-static const int FTM_FAILURE_BIT = BIT1;
-static wifi_ftm_report_entry_t *s_ftm_report;
-static uint8_t s_ftm_report_num_entries;
-static uint32_t s_rtt_est, s_dist_est;
-static bool s_ap_started;
-static uint8_t s_ap_channel;
-static uint8_t s_ap_bssid[ETH_ALEN];
-
-const int g_report_lvl =
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_DIAG
- BIT0 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RTT
- BIT1 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_T1T2T3T4
- BIT2 |
-#endif
-#ifdef CONFIG_ESP_FTM_REPORT_SHOW_RSSI
- BIT3 |
-#endif
- 0;
-
-uint16_t g_scan_ap_num;
-wifi_ap_record_t *g_ap_list_buffer;
-
-// Distance function declaration
-
-double distance(point_t a, point_t b) {
- double dx = a.x - b.x;
- double dy = a.y - b.y;
- double dz = a.z - b.z;
- return sqrt(dx * dx + dy * dy + dz * dz);
-}
-
-// Takes the known positions of the points as arguments and the distances from
-// the unknown point to the known points as arguments
-
-point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4, double d1,
- double d2, double d3, double d4) {
- double x1 = p1.x, y1 = p1.y, z1 = p1.z;
- double x2 = p2.x, y2 = p2.y, z2 = p2.z;
- double x3 = p3.x, y3 = p3.y, z3 = p3.z;
- double x4 = p4.x, y4 = p4.y, z4 = p4.z;
-
- double A1 = 2 * (x2 - x1);
- double B1 = 2 * (y2 - y1);
- double C1 = 2 * (z2 - z1);
- double D1 = d1 * d1 - d2 * d2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2 -
- z1 * z1 + z2 * z2;
-
- double A2 = 2 * (x3 - x1);
- double B2 = 2 * (y3 - y1);
- double C2 = 2 * (z3 - z1);
- double D2 = d1 * d1 - d3 * d3 - x1 * x1 + x3 * x3 - y1 * y1 + y3 * y3 -
- z1 * z1 + z3 * z3;
-
- double A3 = 2 * (x4 - x1);
- double B3 = 2 * (y4 - y1);
- double C3 = 2 * (z4 - z1);
- double D3 = d1 * d1 - d4 * d4 - x1 * x1 + x4 * x4 - y1 * y1 + y4 * y4 -
- z1 * z1 + z4 * z4;
-
- double D4 = d1 * d1 - d1 * d1 - x1 * x1 + x1 * x1 - y1 * y1 + y1 * y1 -
- z1 * z1 + z1 * z1;
-
- double a = A2 / A1;
- double b = B2 / B1;
- double c = C2 / C1;
- double d = D2 / A1;
- double e = D3 / A1 - a * D1 / A1;
- double f = D3 / C1 - c * D1 / C1;
- double g = B3 / B1 - b;
- double h = D3 / B1 - d * B3 / B1;
- double i = D2 / B1 - a * D1 / B1;
- double j = D4 / C1 - c * D1 / C1;
- double k = D4 / B1 - d * B3 / B1;
- double l = i - a * g;
-
- double m = j - c * g;
- double n = (k - b * m / l) / (e - a * m / l);
- double o = (i - j * n) / l;
- double p = d - a * n - c * o;
-
- point_t result = {n, o, p};
- return result;
-}
+#include "math_3d.h"
+#include "commands.h"
+#include "ftm.h"
static void event_handler(void *arg, esp_event_base_t event_base,
int32_t event_id, void *event_data) {
@@ -221,51 +71,6 @@ static void event_handler(void *arg, esp_event_base_t event_base,
}
}
-static void ftm_process_report(void) {
- int i;
- char *log = NULL;
-
- if (!g_report_lvl)
- return;
-
- log = malloc(200);
- if (!log) {
- ESP_LOGE(TAG_STA, "Failed to alloc buffer for FTM report");
- return;
- }
-
- bzero(log, 200);
- sprintf(log, "%s%s%s%s", g_report_lvl & BIT0 ? " Diag |" : "",
- g_report_lvl & BIT1 ? " RTT |" : "",
- g_report_lvl & BIT2 ? " T1 | T2 | T3 "
- " | T4 |"
- : "",
- g_report_lvl & BIT3 ? " RSSI |" : "");
- ESP_LOGI(TAG_STA, "FTM Report:");
- ESP_LOGI(TAG_STA, "|%s", log);
- for (i = 0; i < s_ftm_report_num_entries; i++) {
- char *log_ptr = log;
-
- bzero(log, 200);
- if (g_report_lvl & BIT0) {
- log_ptr += sprintf(log_ptr, "%6d|", s_ftm_report[i].dlog_token);
- }
- if (g_report_lvl & BIT1) {
- log_ptr += sprintf(log_ptr, "%7u |", s_ftm_report[i].rtt);
- }
- if (g_report_lvl & BIT2) {
- log_ptr += sprintf(log_ptr, "%14llu |%14llu |%14llu |%14llu |",
- s_ftm_report[i].t1, s_ftm_report[i].t2,
- s_ftm_report[i].t3, s_ftm_report[i].t4);
- }
- if (g_report_lvl & BIT3) {
- log_ptr += sprintf(log_ptr, "%6d |", s_ftm_report[i].rssi);
- }
- ESP_LOGI(TAG_STA, "|%s", log);
- }
- free(log);
-}
-
void initialise_wifi(void) {
esp_log_level_set("wifi", ESP_LOG_WARN);
static bool initialized = false;
@@ -291,442 +96,9 @@ void initialise_wifi(void) {
initialized = true;
}
-static bool wifi_cmd_sta_join(const char *ssid, const char *pass) {
- int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
-
- wifi_config_t wifi_config = {0};
-
- strlcpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid));
- if (pass) {
- strlcpy((char *)wifi_config.sta.password, pass,
- sizeof(wifi_config.sta.password));
- }
-
- if (bits & CONNECTED_BIT) {
- s_reconnect = false;
- xEventGroupClearBits(s_wifi_event_group, CONNECTED_BIT);
- ESP_ERROR_CHECK(esp_wifi_disconnect());
- xEventGroupWaitBits(s_wifi_event_group, DISCONNECTED_BIT, 0, 1,
- portTICK_PERIOD_MS);
- }
-
- s_reconnect = true;
- s_retry_num = 0;
- ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
- ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
- ESP_ERROR_CHECK(esp_wifi_connect());
-
- xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1,
- 5000 / portTICK_PERIOD_MS);
-
- return true;
-}
-
-static int wifi_cmd_sta(int argc, char **argv) {
- int nerrors = arg_parse(argc, argv, (void **)&sta_args);
-
- if (nerrors != 0) {
- arg_print_errors(stderr, sta_args.end, argv[0]);
- return 1;
- }
-
- ESP_LOGI(TAG_STA, "sta connecting to '%s'", sta_args.ssid->sval[0]);
- wifi_cmd_sta_join(sta_args.ssid->sval[0], sta_args.password->sval[0]);
- return 0;
-}
-
-static bool wifi_perform_scan(const char *ssid, bool internal) {
- wifi_scan_config_t scan_config = {0};
- scan_config.ssid = (uint8_t *)ssid;
- uint8_t i;
-
- ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
- if (ESP_OK != esp_wifi_scan_start(&scan_config, true)) {
- ESP_LOGI(TAG_STA, "Failed to perform scan");
- return false;
- }
-
- esp_wifi_scan_get_ap_num(&g_scan_ap_num);
- if (g_scan_ap_num == 0) {
- ESP_LOGI(TAG_STA, "No matching AP found");
- return false;
- }
-
- if (g_ap_list_buffer) {
- free(g_ap_list_buffer);
- }
- g_ap_list_buffer = malloc(g_scan_ap_num * sizeof(wifi_ap_record_t));
- if (g_ap_list_buffer == NULL) {
- ESP_LOGE(TAG_STA, "Failed to malloc buffer to print scan results");
- return false;
- }
-
- if (esp_wifi_scan_get_ap_records(
- &g_scan_ap_num, (wifi_ap_record_t *)g_ap_list_buffer) == ESP_OK) {
- if (!internal) {
- for (i = 0; i < g_scan_ap_num; i++) {
- ESP_LOGI(TAG_STA,
- "[%s][rssi=%d]"
- "%s",
- g_ap_list_buffer[i].ssid, g_ap_list_buffer[i].rssi,
- g_ap_list_buffer[i].ftm_responder ? "[FTM Responder]" : "");
- }
- }
- }
-
- ESP_LOGI(TAG_STA, "sta scan done");
-
- return true;
-}
-
-static int wifi_cmd_scan(int argc, char **argv) {
- int nerrors = arg_parse(argc, argv, (void **)&scan_args);
-
- if (nerrors != 0) {
- arg_print_errors(stderr, scan_args.end, argv[0]);
- return 1;
- }
-
- ESP_LOGI(TAG_STA, "sta start to scan");
- if (scan_args.ssid->count == 1) {
- wifi_perform_scan(scan_args.ssid->sval[0], false);
- } else {
- wifi_perform_scan(NULL, false);
- }
- return 0;
-}
-
-static bool wifi_cmd_ap_set(const char *ssid, const char *pass) {
- s_reconnect = false;
- strlcpy((char *)g_ap_config.ap.ssid, ssid, MAX_SSID_LEN);
- if (pass) {
- if (strlen(pass) != 0 && strlen(pass) < 8) {
- s_reconnect = true;
- ESP_LOGE(TAG_AP, "password cannot be less than 8 characters long");
- return false;
- }
- strlcpy((char *)g_ap_config.ap.password, pass, MAX_PASSPHRASE_LEN);
- }
-
- if (strlen(pass) == 0) {
- g_ap_config.ap.authmode = WIFI_AUTH_OPEN;
- }
-
- ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
- ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
- return true;
-}
-
-static int wifi_cmd_ap(int argc, char **argv) {
- int nerrors = arg_parse(argc, argv, (void **)&ap_args);
-
- if (nerrors != 0) {
- arg_print_errors(stderr, ap_args.end, argv[0]);
- return 1;
- }
-
- if (true == wifi_cmd_ap_set(ap_args.ssid->sval[0], ap_args.password->sval[0]))
- ESP_LOGI(
- TAG_AP,
- "Starting SoftAP with FTM Responder support, SSID - %s, Password - %s",
- ap_args.ssid->sval[0], ap_args.password->sval[0]);
- else
- ESP_LOGE(TAG_AP, "Failed to start SoftAP!");
-
- return 0;
-}
-
-static int wifi_cmd_query(int argc, char **argv) {
- wifi_config_t cfg;
- wifi_mode_t mode;
-
- esp_wifi_get_mode(&mode);
- if (WIFI_MODE_AP == mode) {
- esp_wifi_get_config(WIFI_IF_AP, &cfg);
- ESP_LOGI(TAG_AP, "AP mode, %s %s", cfg.ap.ssid, cfg.ap.password);
- } else if (WIFI_MODE_STA == mode) {
- int bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
- if (bits & CONNECTED_BIT) {
- esp_wifi_get_config(WIFI_IF_STA, &cfg);
- ESP_LOGI(TAG_STA, "sta mode, connected %s", cfg.ap.ssid);
- } else {
- ESP_LOGI(TAG_STA, "sta mode, disconnected");
- }
- } else {
- ESP_LOGI(TAG_STA, "NULL mode");
- return 0;
- }
-
- return 0;
-}
-
-wifi_ap_record_t *find_ftm_responder_ap(const char *ssid) {
- bool retry_scan = false;
- uint8_t i;
-
- if (!ssid)
- return NULL;
-
-retry:
- if (!g_ap_list_buffer || (g_scan_ap_num == 0)) {
- ESP_LOGI(TAG_STA, "Scanning for %s", ssid);
- if (false == wifi_perform_scan(ssid, true)) {
- return NULL;
- }
- }
-
- for (i = 0; i < g_scan_ap_num; i++) {
- if (strcmp((const char *)g_ap_list_buffer[i].ssid, ssid) == 0)
- return &g_ap_list_buffer[i];
- }
-
- if (!retry_scan) {
- retry_scan = true;
- if (g_ap_list_buffer) {
- free(g_ap_list_buffer);
- g_ap_list_buffer = NULL;
- }
- goto retry;
- }
-
- ESP_LOGI(TAG_STA, "No matching AP found");
-
- return NULL;
-}
-
-static int wifi_cmd_ftm(int argc, char **argv) {
- int nerrors = arg_parse(argc, argv, (void **)&ftm_args);
- wifi_ap_record_t *ap_record;
- EventBits_t bits;
-
- wifi_ftm_initiator_cfg_t ftmi_cfg = {
- .frm_count = 32,
- .burst_period = 2,
- };
-
- if (nerrors != 0) {
- arg_print_errors(stderr, ftm_args.end, argv[0]);
- return 0;
- }
-
- if (ftm_args.initiator->count != 0 && ftm_args.responder->count != 0) {
- ESP_LOGE(TAG_STA, "Invalid FTM cmd argument");
- return 0;
- }
-
- if (ftm_args.responder->count != 0)
- goto ftm_responder;
-
- bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
- if (bits & CONNECTED_BIT && !ftm_args.ssid->count) {
- memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
- ftmi_cfg.channel = s_ap_channel;
- } else if (ftm_args.ssid->count == 1) {
- ap_record = find_ftm_responder_ap(ftm_args.ssid->sval[0]);
- if (ap_record) {
- memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
- ftmi_cfg.channel = ap_record->primary;
- } else {
- return 0;
- }
- } else {
- ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
- return 0;
- }
-
- if (ftm_args.frm_count->count != 0) {
- uint8_t count = ftm_args.frm_count->ival[0];
- if (count != 0 && count != 8 && count != 16 && count != 24 && count != 32 &&
- count != 64) {
- ESP_LOGE(TAG_STA,
- "Invalid Frame Count! Valid options are 0/8/16/24/32/64");
- return 0;
- }
- ftmi_cfg.frm_count = count;
- }
-
- if (ftm_args.burst_period->count != 0) {
- if (ftm_args.burst_period->ival[0] >= 2 &&
- ftm_args.burst_period->ival[0] < 256) {
- ftmi_cfg.burst_period = ftm_args.burst_period->ival[0];
- } else {
- ESP_LOGE(TAG_STA, "Invalid Burst Period! Valid range is 2-255");
- return 0;
- }
- }
-
- ESP_LOGI(TAG_STA,
- "Requesting FTM session with Frm Count - %d, Burst Period - %dmSec "
- "(0: No Preference)",
- ftmi_cfg.frm_count, ftmi_cfg.burst_period * 100);
-
- if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
- ESP_LOGE(TAG_STA, "Failed to start FTM session");
- return 0;
- }
-
- bits =
- xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
- pdTRUE, pdFALSE, portMAX_DELAY);
- /* Processing data from FTM session */
- if (bits & FTM_REPORT_BIT) {
- ftm_process_report();
- free(s_ftm_report);
- s_ftm_report = NULL;
- s_ftm_report_num_entries = 0;
- ESP_LOGI(TAG_STA,
- "Estimated RTT - %d nSec, Estimated Distance - %d.%02d meters",
- s_rtt_est, s_dist_est / 100, s_dist_est % 100);
- } else {
- /* Failure case */
- }
-
- return 0;
-
-ftm_responder:
- if (ftm_args.offset->count != 0) {
- int16_t offset_cm = ftm_args.offset->ival[0];
-
- esp_wifi_ftm_resp_set_offset(offset_cm);
- }
-
- if (ftm_args.enable->count != 0) {
- if (!s_ap_started) {
- ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
- return 0;
- }
- g_ap_config.ap.ftm_responder = true;
- ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
- ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder enabled");
-
- return 0;
- }
-
- if (ftm_args.disable->count != 0) {
- if (!s_ap_started) {
- ESP_LOGE(TAG_AP, "Start the SoftAP first with 'ap' command");
- return 0;
- }
- g_ap_config.ap.ftm_responder = false;
- ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &g_ap_config));
- ESP_LOGI(TAG_AP, "Re-starting SoftAP with FTM Responder disabled");
- }
-
- return 0;
-}
-
-/* Calibration Routine */
-static int wifi_cmd_calibrate(int argc, char **argv) {
- int nerrors = arg_parse(argc, argv, (void **)&calibrate_args);
- wifi_ap_record_t *ap_record;
- EventBits_t bits;
-
- wifi_ftm_initiator_cfg_t ftmi_cfg = {
- .frm_count = 8,
- .burst_period = 2,
- };
-
- if (nerrors != 0) {
- arg_print_errors(stderr, calibrate_args.end, argv[0]);
- return 0;
- }
-
- bits = xEventGroupWaitBits(s_wifi_event_group, CONNECTED_BIT, 0, 1, 0);
- if (bits & CONNECTED_BIT && !calibrate_args.ssid->count) {
- memcpy(ftmi_cfg.resp_mac, s_ap_bssid, ETH_ALEN);
- ftmi_cfg.channel = s_ap_channel;
- } else if (calibrate_args.ssid->count == 1) {
- ap_record = find_ftm_responder_ap(calibrate_args.ssid->sval[0]);
- if (ap_record) {
- memcpy(ftmi_cfg.resp_mac, ap_record->bssid, 6);
- ftmi_cfg.channel = ap_record->primary;
- } else {
- return 0;
- }
- } else {
- ESP_LOGE(TAG_STA, "Provide SSID of the AP in disconnected state!");
- return 0;
- }
-
- /**
- * Check if actual distance is set
- */
-
- if (calibrate_args.actual_distance->count != 0) {
- if (calibrate_args.actual_distance->dval[0] < 0) {
- ESP_LOGE(TAG_STA, "Invalid distance, please enter a positive value");
- return 0;
- }
- }
-
- /**
- * Check if IP id is correct
- */
-
- if (calibrate_args.ap_id->count != 0) {
- uint8_t ap_id = calibrate_args.ap_id->ival[0];
- if (ap_id != 0 && ap_id != 1 && ap_id != 2 && ap_id != 3) {
- ESP_LOGE(TAG_STA, "Invalid AP id. Valid options are 0/1/2/3");
- return 0;
- }
- } else {
- ESP_LOGE(TAG_STA,
- "Please provide an AP id to calibrate. Valid options are 0/1/2/3");
- return 0;
- }
-
- ESP_LOGI(TAG_STA,
- "Calibration Routine with %s\nRequesting FTM session with Frm Count "
- "- %d, Burst Period - %dmSec (0: No Preference)",
- calibrate_args.ssid->sval[0], ftmi_cfg.frm_count,
- ftmi_cfg.burst_period * 100);
-
- if (ESP_OK != esp_wifi_ftm_initiate_session(&ftmi_cfg)) {
- ESP_LOGE(TAG_STA, "Failed to start FTM session");
- return 0;
- }
-
- bits =
- xEventGroupWaitBits(s_ftm_event_group, FTM_REPORT_BIT | FTM_FAILURE_BIT,
- pdTRUE, pdFALSE, portMAX_DELAY);
- /* Processing data from FTM session */
- if (bits & FTM_REPORT_BIT) {
- ftm_process_report();
- free(s_ftm_report);
- s_ftm_report = NULL;
- s_ftm_report_num_entries = 0;
-
- ESP_LOGI(TAG_STA, "-------- CALIBRATION --------");
-
- calibration_offsets[calibrate_args.ap_id->ival[0]] =
- (calibrate_args.actual_distance->dval[0] * 100 - s_dist_est) / 100;
-
- ESP_LOGI(TAG_STA, "Calibrating AP_%d", calibrate_args.ap_id->ival[0]);
- ESP_LOGI(TAG_STA, "Estimated RTT : %d nSec\n", s_rtt_est);
-
- ESP_LOGI(TAG_STA, "Estimated distance (no calibration) : %d.%02d",
- s_dist_est / 100, s_dist_est % 100);
- ESP_LOGI(TAG_STA, "Actual distance : %.2f",
- calibrate_args.actual_distance->dval[0]);
- ESP_LOGI(TAG_STA, "Calibration offset : %.2f",
- calibration_offsets[calibrate_args.ap_id->ival[0]]);
-
- } else {
- /* Failure case */
- }
-
- return 0;
-}
-
-static int wifi_cmd_list_offsets(int argc, char **argv) {
- ESP_LOGI(TAG_STA, "-------- OFFSETS --------");
- for (size_t i = 0; i < 4; i++) {
- ESP_LOGI(TAG_STA, "AP_%d : %.2f", i, calibration_offsets[i]);
- }
-
- return 0;
-}
-
+/*
+ * Register the FTM related commands
+ */
void register_wifi(void) {
sta_args.ssid = arg_str1(NULL, NULL, "<ssid>", "SSID of AP");
sta_args.password = arg_str0(NULL, NULL, "<pass>", "password of AP");
@@ -863,4 +235,4 @@ void app_main(void) {
// start console REPL
ESP_ERROR_CHECK(esp_console_start_repl(repl));
-}
+}
\ No newline at end of file
diff --git a/main/math_3d.c b/main/math_3d.c
new file mode 100644
index 0000000000000000000000000000000000000000..327739b340cf9433dcb4f93e542f763eaede4efc
--- /dev/null
+++ b/main/math_3d.c
@@ -0,0 +1,59 @@
+#include "math_3d.h"
+#include <math.h>
+
+double distance(point_t a, point_t b) {
+ double dx = a.x - b.x;
+ double dy = a.y - b.y;
+ double dz = a.z - b.z;
+ return sqrt(dx * dx + dy * dy + dz * dz);
+}
+
+point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4,
+ double d1, double d2, double d3, double d4) {
+ double x1 = p1.x, y1 = p1.y, z1 = p1.z;
+ double x2 = p2.x, y2 = p2.y, z2 = p2.z;
+ double x3 = p3.x, y3 = p3.y, z3 = p3.z;
+ double x4 = p4.x, y4 = p4.y, z4 = p4.z;
+
+ double A1 = 2 * (x2 - x1);
+ double B1 = 2 * (y2 - y1);
+ double C1 = 2 * (z2 - z1);
+ double D1 = d1 * d1 - d2 * d2 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2 -
+ z1 * z1 + z2 * z2;
+
+ double A2 = 2 * (x3 - x1);
+ double B2 = 2 * (y3 - y1);
+ double C2 = 2 * (z3 - z1);
+ double D2 = d1 * d1 - d3 * d3 - x1 * x1 + x3 * x3 - y1 * y1 + y3 * y3 -
+ z1 * z1 + z3 * z3;
+
+ double A3 = 2 * (x4 - x1);
+ double B3 = 2 * (y4 - y1);
+ double C3 = 2 * (z4 - z1);
+ double D3 = d1 * d1 - d4 * d4 - x1 * x1 + x4 * x4 - y1 * y1 + y4 * y4 -
+ z1 * z1 + z4 * z4;
+
+ double D4 = d1 * d1 - d1 * d1 - x1 * x1 + x1 * x1 - y1 * y1 + y1 * y1 -
+ z1 * z1 + z1 * z1;
+
+ double a = A2 / A1;
+ double b = B2 / B1;
+ double c = C2 / C1;
+ double d = D2 / A1;
+ double e = D3 / A1 - a * D1 / A1;
+ double f = D3 / C1 - c * D1 / C1;
+ double g = B3 / B1 - b;
+ double h = D3 / B1 - d * B3 / B1;
+ double i = D2 / B1 - a * D1 / B1;
+ double j = D4 / C1 - c * D1 / C1;
+ double k = D4 / B1 - d * B3 / B1;
+ double l = i - a * g;
+
+ double m = j - c * g;
+ double n = (k - b * m / l) / (e - a * m / l);
+ double o = (i - j * n) / l;
+ double p = d - a * n - c * o;
+
+ point_t result = {n, o, p};
+ return result;
+}
\ No newline at end of file
diff --git a/main/math_3d.h b/main/math_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a0d4808be1df8ac22d563787726d65c8c1455f6
--- /dev/null
+++ b/main/math_3d.h
@@ -0,0 +1,21 @@
+#pragma once
+
+/*
+ * A 3D point in cartesian coordinates.
+ */
+typedef struct {
+ double x;
+ double y;
+ double z;
+} point_t;
+
+/*
+ * Computes the distance between two 3D points.
+ */
+double distance(point_t a, point_t b);
+
+/*
+ *
+ */
+point_t triangulate(point_t p1, point_t p2, point_t p3, point_t p4,
+ double d1, double d2, double d3, double d4);
\ No newline at end of file