基于wlan_rssi无线定位源码解析

Wlan定位之三点定位分析：

         本文完全是根据实际开发环境所写，我们这套系统由定位设备和定位服务器组成，之间通信采用私有协议，定位设备采用傲天的ap,定位服务器是我司IPS设备

*注释 ：以下定位设备称为sensor，定位服务器成为IPS,被定为目标称为DDev

sensor主要工作：采集数据，计算距离

IPS主要工作：管理探针，计算被定位设备坐标，定位页面图形展示。

 

用户态采集数据：sensor每100ms采集DDev  的Beacon 或者Probe response帧等的信号强度，一共30组数据

计算距离：30组数据经过高斯滤波算法得到平均的RSSI,代入公式 rssi = -(10nlgd +A),其中n为环境因子，A为sensor距离DDev一米采集到的DDev的信号强度

源码如下：

/*算法实现函数*/

float rtlsCalcAverageRssi(char *pstRssi, char n)

{

         int k, l = 0;

         char i , j, m, mm;

         float perRssi;

         float  AverageRssi = 0;

         float sigma = 0, mu = 0;

 

         //calc mu

         for(i = 0; i < n; i++)

         {

                   perRssi = pstRssi[i];

                   mu += perRssi;

         }

         mu = mu/n;

 

         //calc sigma

         for(j = 0; j < n; j++)

         {

                   perRssi = pstRssi[j] - mu;

                   perRssi = pow(perRssi, 2);

                   sigma += perRssi;

         }       

         sigma = sigma/(n -1);

         sigma = pow(sigma, 0.5);

         _sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : mu = %f , sigma = %f__", mu, sigma);

         float lowerLimit = 0.15*sigma + mu;//既有公式

         //lowerLimit = (int)lowerLimit;

         //printf("lowerLimit = %d\n", (int)lowerLimit);

 

         float upperLimit = 3.09*sigma + mu;//既有公式

         //upperLimit = (int)upperLimit;

         //printf("upperLimit = %d\n", (int)upperLimit);

         _sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : lowerLimit = %f , upperLimit = %f__", lowerLimit, upperLimit);

 

         for(m = 0; m < n; m++)

         {

                   mm = pstRssi[m];

                   if((mm  >= (int)lowerLimit)&&(mm  <= (int)upperLimit))

                   {

                            ++l;

                            AverageRssi += mm;                

                            //_sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : l = ", l);

                   }

         }

 

         if(l)

         {

                   _sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : count = %d", l);

                   AverageRssi = AverageRssi/l;

                   return AverageRssi;

         }

        

         return mu;

}

/*发送距离给IPS*/

VOID  sensorCalc(sensorLocalDev *pstLocalDev)

{

         struct list_head  *listAP;

         struct list_head  *listn;

         sensorLocalDev *psttmpLocalDev = NULL;

         char i= 0;

         UCHAR sample_rssi[130] = {0};

         UCHAR sample_rssi_s[4] = {0};

 

         _sensor_printf(g_sensordDebug, MSG_BASIC, "__sensor start calc ......");

         for(i = 0; i < SENSOR_SAMPLE_COUNT; i++)

         {

                   snprintf(sample_rssi_s, 4, "%d ", pstLocalDev->sampleRssi[i]);

                   strncat(sample_rssi, sample_rssi_s, 4);

         }

         _sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : sample_rssi = %s", sample_rssi);

 

         float rssi =0;

         rssi = rtlsCalcAverageRssi(pstLocalDev->sampleRssi, SENSOR_SAMPLE_COUNT);

         float distance;

         if((g_sensorLocalpara.para_A)&&(g_sensorLocalpara.para_N))

         {

                   distance = rtlsCalcDiance(rssi, g_sensorLocalpara.para_A, g_sensorLocalpara.para_N);

         }

         else

                  distance = rtlsCalcDiance(rssi, pstLocalDev->para_A, pstLocalDev->para_N);

         _sensor_printf(g_sensordDebug, MSG_BASIC, "__Localtion : rssi = %f , distance = %f__", rssi, distance);

 

         //send to idp

         sensorSendLocalinfo(distance, pstLocalDev);

         return;

}

/*定时采集信号强度函数*/

VOID  sensorSampleRssi(void *eloop_ctx, void *timeout_ctx)

{

         struct AP_info *ap_cur = NULL;

         sensorLocalDev *pstLocalDev = (sensorLocalDev *)eloop_ctx;                                                   

         ap_cur = G.ap_1st;

         while( ap_cur != NULL )

         {

                   if( (ap_cur->nb_pkt < 2)  || !memcmp( ap_cur->bssid, BROADCAST, WIDP_SENTOR_MAC_LEN ) ||(ap_cur->avg_power >= -1)  ||

                   !memcmp( ap_cur->bssid, NULL_MAC, WIDP_SENTOR_MAC_LEN ))

                   {

                            ap_cur = ap_cur->next;

                            continue;

                   }

 

                   if(!memcmp( ap_cur->bssid, pstLocalDev->LocalMacAddr, WIDP_SENTOR_MAC_LEN ))

                   {

                            _sensor_printf(g_sensordDebug, MSG_BASIC, "__LocalMacAddr is exist in the g_linktable__ !");

                            if(pstLocalDev->sampleCount >= SENSOR_SAMPLE_COUNT)

                            {

                                     pstLocalDev->sampleCount = 0;     

                                     sensorCalc(pstLocalDev);

                                     //return;

                            }

                            else

                            {

                                     _sensor_printf(g_sensordDebug, MSG_BASIC, "__local power == %d", ap_cur->avg_power);

                                     //if((ap_cur->avg_power <= -20)&&(ap_cur->avg_power >= -80))

                                     {

                                               pstLocalDev->sampleRssi[pstLocalDev->sampleCount] = ap_cur->avg_power;

                                               pstLocalDev->sampleCount++;

                                     }

                            }

                            break;

                   }

                   ap_cur = ap_cur->next;

         }

         eloop_register_timeout(0, 100*1000,  sensorSampleRssi, pstLocalDev, NULL);        

         return;

}

 

计算被定位设备坐标：IPS负责计算DDev坐标，如果要计算精确的DDev坐标，需要三个sensor,IPS负责管理这些sensor,包括sensor坐标的设置，以及页面的动态显示，主要调用highcharts.js和highcharts-more.js插件并经过自己的修改完成的，有兴趣的哥们姐们可以研究一下这两个插件，我在这里不做分析。

刚才说到sensor会发送距离d给IPS，IPS会做进一步的处理,那究竟是什么处理呢，我们在这里详细说说.

/*对d的值进行啦一次高斯滤波算法*/

int Gaussian_distribution(unsigned short *array, int n)

{

         int k=0;

         int i;

         float perRssi;

         int tmp;

         int  Average = 0;

         float sigma = 0, mu = 0;

 

         //calc mu

         for(i = 0; i < n; i++)

         {

         //      printf("[%d] %d  \n",i, array[i]);

                   perRssi = (float)array[i];

                   mu += perRssi;

         }

         mu = mu/n;

 

         //calc sigma

         for(i = 0; i < n; i++)

         {

                   perRssi = (float)array[i] - mu;

                   perRssi = pow(perRssi, 2);

                   sigma += perRssi;

         }

        

         sigma = sigma/(n -1);

         sigma = pow(sigma, 0.5);

 

         int lowerLimit = 0.15*sigma + mu;//既有公式

         //      printf("lowerLimit = %d\n", (int)lowerLimit);

 

         int upperLimit = 3.09*sigma + mu;//既有公式

         //      printf("upperLimit = %d\n", (int)upperLimit);

        

         for(i = 0; i < n; i++)

         {

                   tmp = array[i];

                   if((tmp >= lowerLimit) && (tmp <= upperLimit))

                   {

                            k++;

                            Average += tmp;

                   }

         }

         if( k > 0 )

         {

                   Average = Average/k;

         }

         else

         {

                   Average = mu;

         }

 

         return Average;

}

/*IPS 获取d函数*/

int    widp_get_ap_location(struct widp_cmd_msg_head *mhead)

{

         struct list_head *pos;

         int len = 0;

         char buf[1024];

         struct widp_cmd_msg_head *rcv_head = (struct widp_cmd_msg_head*)buf;    

         char *msg = buf + sizeof(struct widp_cmd_msg_head);        

         int id = mhead->area_id;

         struct widp_location_ask_info *apinfo = (struct widp_location_ask_info *)(mhead->msg);

         struct widp_ap_location_node *locaion_node;

         int i;

         float dis = 0;

 

         /*get all location info*/

         if(apinfo->num >= 0)

         {

                   return widp_get_ap_location_all(apinfo->num);

         }

        

         rcv_head->area_id = id;

         rcv_head->cmd_type = WIDP_RCV_MSG;

        

         pthread_mutex_lock(&widp_positioning_ap_list.lock);

         list_for_each( pos, &widp_positioning_ap_list.l)

         {

                   locaion_node= list_entry( pos, struct widp_ap_location_node, h);

                   if(MAC_IS_EQUAL(apinfo->bssid.mac, locaion_node->ap_bssid.mac))

                   {

                            for(i=0 ; i < WIDP_SENSOR_MAX; i++)

                            {

                                     if(mac_zero(locaion_node->location[i].sensor.mac))

                                     {

                                               break;

                                     }

 

                                     if(locaion_node->location[i].dis[MAX_DISTANCE_NUM - 1])

                                     {

                                               dis = Gaussian_distribution(locaion_node->location[i].dis, MAX_DISTANCE_NUM);

                                     }

                                     else if(locaion_node->location[i].index > 1)

                                     {

                                               dis = Gaussian_distribution(locaion_node->location[i].dis, locaion_node->location[i].index);

                                     }

                                     else

                                     {

                                               dis = locaion_node->location[i].dis[0];

                                     }

 

                                     printf("Gaussian_distribution dis %f\n", dis);

 

                                     len += PRINTF_MAC_TO_STR(msg+len,locaion_node->location[i].sensor.mac);                              

                                     if(locaion_node->location[i].name[0])

                                     {

                                               len += sprintf(msg+len, "|%s",locaion_node->location[i].name);

                                     }

                                     else

                                     {

                                               len += sprintf(msg+len, "|null");

                                     }

                                     len += sprintf(msg+len, "|(%d,%d)", locaion_node->location[i].coordinate_x, locaion_node->location[i].coordinate_y);       

                                     if(dis > WIDP_AP_DISTANCE_MAX)

                                     {

                                               len += sprintf(msg+len, "|>100\n");       

                                     }

                                     else

                                     {

                                               len += sprintf(msg+len, "|%d\n",(int)dis);      

                                     }

                            }

                            //memset(apn->location, 0 , WIDP_SENSOR_MAX* sizeof(struct location_info));

                            break;

                   }

         }

         pthread_mutex_unlock(&widp_positioning_ap_list.lock);

 

         if(len == 0)

         {

                   len += sprintf(msg+len, "NO LOCATION INFO\n");

         }

         msg[len++] = 0;

         rcv_head->msg_len = len;

         len += sizeof(struct widp_cmd_msg_head);  

         send_to_cmd_client(buf, len);

        

         return 0;

}