This program calculate location of Wi-Fi receiver, by using AP beacon. Please check the Japanese magazine "Interface 2012/12".
LocationHandler.h
00001 #include <math.h> 00002 #include <string.h> 00003 00004 #define NOTFOUND_MARK (-65536) 00005 00006 struct apinfo registered_ap[] = { 00007 { "ap-game-432afd", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfd}, 0 }, 00008 { "ap-pc-432afc", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfc}, 0 }, 00009 { "ap-game-433f85", {0, 0x22, 0xcf, 0x43, 0x3f, 0x85}, 0 }, 00010 { "ap-pc-433f84", {0, 0x22, 0xcf, 0x43, 0x3f, 0x84}, 0 }, 00011 { "ap-game-433223", {0, 0x22, 0xcf, 0x43, 0x32, 0x23}, 0 }, 00012 { "ap-pc-433222", {0, 0x22, 0xcf, 0x43, 0x32, 0x22}, 0 }, 00013 { "ap-game-4334c9", {0, 0x22, 0xcf, 0x43, 0x34, 0xc9}, 0 }, 00014 { "ap-pc-4334c8", {0, 0x22, 0xcf, 0x43, 0x34, 0xc8}, 0 } 00015 }; // this structure must be writable 00016 00017 struct pos { 00018 float x, y; 00019 }; 00020 00021 const struct pos registered_pos[] = { 00022 {0.0, 0.0}, {0.0, 0.0}, {0.0, 100.0}, {0.0, 100.0}, 00023 {100.0, 0.0}, {100.0, 0.0}, {100.0, 100.0}, {100.0, 100.0} 00024 }; 00025 00026 struct pos result_pos; 00027 00028 void LocationManagerCalcPos(int apinfo_count, struct apinfo* searched_ap) 00029 { 00030 int i, j, k; 00031 int match_count; 00032 const int length_of_registered_ap = sizeof(registered_ap) / sizeof(struct apinfo); 00033 float proportion; 00034 float sum; 00035 00036 for ( i = 0; i < length_of_registered_ap; i++ ) { 00037 registered_ap[i].power = NOTFOUND_MARK; 00038 } 00039 match_count = 0; 00040 for ( i = 0; i < apinfo_count; i++ ) { 00041 for ( j = 0; j < length_of_registered_ap; j++ ) { 00042 for ( k = 0; k < ESSID_LEN; k++ ) { 00043 if ( registered_ap[j].essid[k] == '\0' ) { 00044 break; 00045 } 00046 if ( searched_ap[i].essid[k] != registered_ap[j].essid[k] ) { 00047 goto no_match; 00048 } 00049 } 00050 if ( memcmp( registered_ap[j].bssid, searched_ap[i].bssid, MACADDR_LEN) != 0 ) { 00051 continue; 00052 } 00053 registered_ap[j].power = searched_ap[i].power; 00054 match_count++; 00055 break; 00056 no_match:; 00057 } 00058 } 00059 00060 if ( match_count == 0 ) { 00061 lcd.cls(); 00062 lcd.printf("location lost\nAP count %d", apinfo_count); 00063 return; 00064 } 00065 00066 result_pos.x = result_pos.y = 0.0; 00067 sum = 0.0; 00068 for ( i = 0; i < length_of_registered_ap; i++ ) { 00069 proportion = pow(10, registered_ap[i].power / 40.0f); // calc dB to linear number 00070 sum += proportion; 00071 result_pos.x += registered_pos[i].x * proportion; 00072 result_pos.y += registered_pos[i].y * proportion; 00073 } 00074 result_pos.x /= sum; 00075 result_pos.y /= sum; 00076 00077 lcd.cls(); 00078 lcd.printf("AP %d/%d, location x=%.1f, y=%.1f", match_count, apinfo_count, result_pos.x, result_pos.y); 00079 }
Generated on Tue Jul 12 2022 20:37:42 by 1.7.2