https://blog.csdn.net/PINGER0077/article/details/79482238
ESP8266不需要修改任何庫
#include "ESP8266WiFi.h"
#include "math.h"
/*
已知三點位置 (x1, y1,z1), (x2, y2,z2), (x3, y3,z3)
已知未知點 (x, y,z) 到三點距離 d1, d2, d3
以 d1, d2, d3 為半徑作三個圓，根據畢達哥拉斯定理，得出交點即未知點的位置計算公式：
( x1 - x0 )2 + ( y1 - y0 )2 = d12
( x2 - x0 )2 + ( y2 - y0 )2 = d22
( x3 - x0 )2 + ( y3 - y0 )2 = d32
設未知點位置為 (x, y)， 令其中的第一個球形 P1 的球心坐標為 (0, 0)，P2 處於相同縱坐標，球心坐標為 (d, 0)，P3 球心坐標為 (i, j)，
三個球形半徑分別為 r1, r2, r3，z為三球形相交點與水平面高度。則有：
r12 = x2 + y2 + z2
r22 = (x - d)2 + y2 + z2
r32 = (x - i)2 + (y - j)2 + z2
化簡
x = (r12 - r22 + d2) / 2d
y = (r12 - r32 - x2 + (x - i)2 + j2) / 2j
*/
//第一個點（不修改） 原點 第一個球形 P1 的球心坐標為 (0, 0)
#define x1 0-----------------------------------------------------
#define y1 0-----------------------------------------------------
#define z1 0 //高度一致------------------------------------------
//第二個點（只修改y2） P2 處於相同縱坐標，球心坐標為 (d, 0)------
float x2 =0 ; //實際位置距離測量 1-20米-------------------------- 修改
float y2 =0 ;//實際位置放在相對原點縱坐標為0 --------------------
float z2 =0 ;//高度一致------------------------------------------
//第三個WIfi距離 (修改x3和y3)
float x3 =0; // 實際位置測量------------------------------------修改
float y3 =0; // 實際位置測量------------------------------------修改
float z3 =0; //高度一致-----------------------------------------
// 物體實際位置
float x=0;
float y=0;
float z=0;
//測量距離
float d1=0;
float d2=0;
float d3=0;
/*
1 單個距離公式
d = 10^((abs(rssi) - A) / (10 * n)) 信號強度
2 參數介紹
d - 計算所得距離(單位：m)
rssi - 接收信號強度
A - 發射端和接收端相隔1米時的信號強度
n - 環境衰減因子
*/
// 3 在自己的環境里實際測試，修改下面兩個值適應環境
#define wifiname1 "gps1"
#define A1 -42 //接收機和發射機間隔1m時的信號強度 ssid 一般45-52
#define N1 40 //N = 10 * n ,其中n為環境衰減因子，3.25-4.5 這個沒有專門設備無法測到，只能在A數值確定了，修改這個數值，使得輸出距離為 1米
#define wifiname2 "gps2"
#define A2 -42 //接收機和發射機間隔1m時的信號強度 ssid 一般45-52
#define N2 40 //N = 10 * n ,其中n為環境衰減因子，3.25-4.5 這個沒有專門設備無法測到，只能在A數值確定了，修改這個數值，使得輸出距離為 1米
#define wifiname3 "gps3"
#define A3 -42 //接收機和發射機間隔1m時的信號強度 ssid 一般45-52
#define N3 40 //N = 10 * n ,其中n為環境衰減因子，3.25-4.5 這個沒有專門設備無法測到，只能在A數值確定了，修改這個數值，使得輸出距離為 1米
//信號臨界值強大要達到 -65 才參與運算
int rssi_min=-65;
//至少需要 3 個 ap都檢測到
int ap_number=0;
int ap_number_min=3;
void setup() {
Serial.begin(115200);
WiFi.mode(WIFI_STA);
WiFi.disconnect();
delay(100);
Serial.println("Setup done");
}
// 獲取單個WIfi距離
float Distance(String wifissid,int wifirssi,int A,int N){
if(wifissid=="gps1"){
Serial.print("Wifiname:"); Serial.print(wifissid); Serial.print(" RSSI:"); Serial.print(wifirssi); Serial.print("");
float iu, distance;
iu = (float)(A-wifirssi ) / (float)N;
distance = powf(10.0, iu);//計算以x為底數的y次冪 功能與pow一致，只是輸入與輸出皆為浮點數
Serial.print(" Distance:"); Serial.print(distance); Serial.println("m");
return distance;
}
}
//獲取自身位置 x y坐標
/*
輸入參數
float r1 設備距離第一個wifi的距離
float r2 設備距離第二個wifi的距離
float r3 設備距離第三個wifi的距離
float x2, 第一個點為圓心，第二個點同一水平位置隨意放，第一個點到第二個點的實際距離
float x3 以第一個點為原點，以第一個和第二個點連線為X軸，垂直其方向為y軸，第三個點x坐標
float y3 以第一個點為原點，以第一個和第二個點連線為X軸，垂直其方向為y軸，第三個點y坐標
*/
//簡單算法
void My_xyz(float r1,float r2,float r3,float x2,float x3, float y3){
float x = (float)(r1*r1 - r2*r2 + x2*x2) / (float)2*x2;
float y = (float)(r1*r1 - r3*r3 - x*x + (x - x3)*(x - x3) + y3*y3) / (float)2*y3;
Serial.print("x:"); Serial.print(x);
Serial.print(" y:"); Serial.println(y);
}
void My_2x(){
}
void loop() {
// Serial.println("scan start");
int n = WiFi.scanNetworks();
Serial.println("---------------------WIFI Scan done---------------------");
if (n == 0) {
Serial.println("no networks found");
} else {
ap_number=0;//檢測到的數目
d1=0;//第一個距離
d2=0;//第二個距離
d3=0;//第三個距離
for (int i = 0; i < n; ++i) {
// Print SSID and RSSI for each network found
if(WiFi.SSID(i)==wifiname1&&WiFi.RSSI(i)>rssi_min){
d1= Distance(WiFi.SSID(i),WiFi.RSSI(i),A1,N1);ap_number++;
}
if(WiFi.SSID(i)==wifiname2&&WiFi.RSSI(i)>rssi_min){
d2= Distance(WiFi.SSID(i),WiFi.RSSI(i),A2,N2);ap_number++;
}
if(WiFi.SSID(i)==wifiname3&&WiFi.RSSI(i)>rssi_min){
d3= Distance(WiFi.SSID(i),WiFi.RSSI(i),A3,N3);ap_number++;
}
}//for
//----------------------------計算位置--------------------------------
if(ap_number==ap_number_min)
{
My_xyz(d1,d2,d3, x2, x3, y3);
}
else{
Serial.println("AP_number < 3,distance get fail!");
}
}
delay(500);
}