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 }