（1）WIFI信号确定距离

#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);
}