This program calculate location of Wi-Fi receiver, by using AP beacon. Please check the Japanese magazine "Interface 2012/12".
LocationHandler.h@1:2f2f793fcf4f, 2012-09-10 (annotated)
- Committer:
- nakata
- Date:
- Mon Sep 10 07:15:35 2012 +0000
- Revision:
- 1:2f2f793fcf4f
- Parent:
- 0:4eaf38ccb19c
for Interface
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakata | 0:4eaf38ccb19c | 1 | #include <math.h> |
nakata | 0:4eaf38ccb19c | 2 | #include <string.h> |
nakata | 0:4eaf38ccb19c | 3 | |
nakata | 0:4eaf38ccb19c | 4 | #define NOTFOUND_MARK (-65536) |
nakata | 0:4eaf38ccb19c | 5 | |
nakata | 0:4eaf38ccb19c | 6 | struct apinfo registered_ap[] = { |
nakata | 0:4eaf38ccb19c | 7 | { "ap-game-432afd", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfd}, 0 }, |
nakata | 0:4eaf38ccb19c | 8 | { "ap-pc-432afc", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfc}, 0 }, |
nakata | 0:4eaf38ccb19c | 9 | { "ap-game-433f85", {0, 0x22, 0xcf, 0x43, 0x3f, 0x85}, 0 }, |
nakata | 0:4eaf38ccb19c | 10 | { "ap-pc-433f84", {0, 0x22, 0xcf, 0x43, 0x3f, 0x84}, 0 }, |
nakata | 0:4eaf38ccb19c | 11 | { "ap-game-433223", {0, 0x22, 0xcf, 0x43, 0x32, 0x23}, 0 }, |
nakata | 0:4eaf38ccb19c | 12 | { "ap-pc-433222", {0, 0x22, 0xcf, 0x43, 0x32, 0x22}, 0 }, |
nakata | 0:4eaf38ccb19c | 13 | { "ap-game-4334c9", {0, 0x22, 0xcf, 0x43, 0x34, 0xc9}, 0 }, |
nakata | 0:4eaf38ccb19c | 14 | { "ap-pc-4334c8", {0, 0x22, 0xcf, 0x43, 0x34, 0xc8}, 0 } |
nakata | 0:4eaf38ccb19c | 15 | }; // this structure must be writable |
nakata | 0:4eaf38ccb19c | 16 | |
nakata | 0:4eaf38ccb19c | 17 | struct pos { |
nakata | 0:4eaf38ccb19c | 18 | float x, y; |
nakata | 0:4eaf38ccb19c | 19 | }; |
nakata | 0:4eaf38ccb19c | 20 | |
nakata | 0:4eaf38ccb19c | 21 | const struct pos registered_pos[] = { |
nakata | 0:4eaf38ccb19c | 22 | {0.0, 0.0}, {0.0, 0.0}, {0.0, 100.0}, {0.0, 100.0}, |
nakata | 0:4eaf38ccb19c | 23 | {100.0, 0.0}, {100.0, 0.0}, {100.0, 100.0}, {100.0, 100.0} |
nakata | 0:4eaf38ccb19c | 24 | }; |
nakata | 0:4eaf38ccb19c | 25 | |
nakata | 0:4eaf38ccb19c | 26 | struct pos result_pos; |
nakata | 0:4eaf38ccb19c | 27 | |
nakata | 0:4eaf38ccb19c | 28 | void LocationManagerCalcPos(int apinfo_count, struct apinfo* searched_ap) |
nakata | 0:4eaf38ccb19c | 29 | { |
nakata | 0:4eaf38ccb19c | 30 | int i, j, k; |
nakata | 0:4eaf38ccb19c | 31 | int match_count; |
nakata | 0:4eaf38ccb19c | 32 | const int length_of_registered_ap = sizeof(registered_ap) / sizeof(struct apinfo); |
nakata | 0:4eaf38ccb19c | 33 | float proportion; |
nakata | 0:4eaf38ccb19c | 34 | float sum; |
nakata | 0:4eaf38ccb19c | 35 | |
nakata | 0:4eaf38ccb19c | 36 | for ( i = 0; i < length_of_registered_ap; i++ ) { |
nakata | 0:4eaf38ccb19c | 37 | registered_ap[i].power = NOTFOUND_MARK; |
nakata | 0:4eaf38ccb19c | 38 | } |
nakata | 0:4eaf38ccb19c | 39 | match_count = 0; |
nakata | 0:4eaf38ccb19c | 40 | for ( i = 0; i < apinfo_count; i++ ) { |
nakata | 0:4eaf38ccb19c | 41 | for ( j = 0; j < length_of_registered_ap; j++ ) { |
nakata | 0:4eaf38ccb19c | 42 | for ( k = 0; k < ESSID_LEN; k++ ) { |
nakata | 0:4eaf38ccb19c | 43 | if ( registered_ap[j].essid[k] == '\0' ) { |
nakata | 0:4eaf38ccb19c | 44 | break; |
nakata | 0:4eaf38ccb19c | 45 | } |
nakata | 0:4eaf38ccb19c | 46 | if ( searched_ap[i].essid[k] != registered_ap[j].essid[k] ) { |
nakata | 0:4eaf38ccb19c | 47 | goto no_match; |
nakata | 0:4eaf38ccb19c | 48 | } |
nakata | 0:4eaf38ccb19c | 49 | } |
nakata | 0:4eaf38ccb19c | 50 | if ( memcmp( registered_ap[j].bssid, searched_ap[i].bssid, MACADDR_LEN) != 0 ) { |
nakata | 0:4eaf38ccb19c | 51 | continue; |
nakata | 0:4eaf38ccb19c | 52 | } |
nakata | 0:4eaf38ccb19c | 53 | registered_ap[j].power = searched_ap[i].power; |
nakata | 0:4eaf38ccb19c | 54 | match_count++; |
nakata | 0:4eaf38ccb19c | 55 | break; |
nakata | 0:4eaf38ccb19c | 56 | no_match:; |
nakata | 0:4eaf38ccb19c | 57 | } |
nakata | 0:4eaf38ccb19c | 58 | } |
nakata | 0:4eaf38ccb19c | 59 | |
nakata | 0:4eaf38ccb19c | 60 | if ( match_count == 0 ) { |
nakata | 0:4eaf38ccb19c | 61 | lcd.cls(); |
nakata | 0:4eaf38ccb19c | 62 | lcd.printf("location lost\nAP count %d", apinfo_count); |
nakata | 0:4eaf38ccb19c | 63 | return; |
nakata | 0:4eaf38ccb19c | 64 | } |
nakata | 0:4eaf38ccb19c | 65 | |
nakata | 0:4eaf38ccb19c | 66 | result_pos.x = result_pos.y = 0.0; |
nakata | 0:4eaf38ccb19c | 67 | sum = 0.0; |
nakata | 0:4eaf38ccb19c | 68 | for ( i = 0; i < length_of_registered_ap; i++ ) { |
nakata | 0:4eaf38ccb19c | 69 | proportion = pow(10, registered_ap[i].power / 40.0f); // calc dB to linear number |
nakata | 0:4eaf38ccb19c | 70 | sum += proportion; |
nakata | 0:4eaf38ccb19c | 71 | result_pos.x += registered_pos[i].x * proportion; |
nakata | 0:4eaf38ccb19c | 72 | result_pos.y += registered_pos[i].y * proportion; |
nakata | 0:4eaf38ccb19c | 73 | } |
nakata | 0:4eaf38ccb19c | 74 | result_pos.x /= sum; |
nakata | 0:4eaf38ccb19c | 75 | result_pos.y /= sum; |
nakata | 0:4eaf38ccb19c | 76 | |
nakata | 0:4eaf38ccb19c | 77 | lcd.cls(); |
nakata | 0:4eaf38ccb19c | 78 | lcd.printf("AP %d/%d, location x=%.1f, y=%.1f", match_count, apinfo_count, result_pos.x, result_pos.y); |
nakata | 0:4eaf38ccb19c | 79 | } |