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