This program calculate location of Wi-Fi receiver, by using AP beacon. Please check the Japanese magazine "Interface 2012/12".
Diff: LocationHandler.h
- Revision:
- 0:4eaf38ccb19c
diff -r 000000000000 -r 4eaf38ccb19c LocationHandler.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LocationHandler.h Fri Aug 24 03:21:43 2012 +0000 @@ -0,0 +1,79 @@ +#include <math.h> +#include <string.h> + +#define NOTFOUND_MARK (-65536) + +struct apinfo registered_ap[] = { + { "ap-game-432afd", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfd}, 0 }, + { "ap-pc-432afc", {0, 0x22, 0xcf, 0x43, 0x2a, 0xfc}, 0 }, + { "ap-game-433f85", {0, 0x22, 0xcf, 0x43, 0x3f, 0x85}, 0 }, + { "ap-pc-433f84", {0, 0x22, 0xcf, 0x43, 0x3f, 0x84}, 0 }, + { "ap-game-433223", {0, 0x22, 0xcf, 0x43, 0x32, 0x23}, 0 }, + { "ap-pc-433222", {0, 0x22, 0xcf, 0x43, 0x32, 0x22}, 0 }, + { "ap-game-4334c9", {0, 0x22, 0xcf, 0x43, 0x34, 0xc9}, 0 }, + { "ap-pc-4334c8", {0, 0x22, 0xcf, 0x43, 0x34, 0xc8}, 0 } +}; // this structure must be writable + +struct pos { + float x, y; +}; + +const struct pos registered_pos[] = { + {0.0, 0.0}, {0.0, 0.0}, {0.0, 100.0}, {0.0, 100.0}, + {100.0, 0.0}, {100.0, 0.0}, {100.0, 100.0}, {100.0, 100.0} +}; + +struct pos result_pos; + +void LocationManagerCalcPos(int apinfo_count, struct apinfo* searched_ap) +{ + int i, j, k; + int match_count; + const int length_of_registered_ap = sizeof(registered_ap) / sizeof(struct apinfo); + float proportion; + float sum; + + for ( i = 0; i < length_of_registered_ap; i++ ) { + registered_ap[i].power = NOTFOUND_MARK; + } + match_count = 0; + for ( i = 0; i < apinfo_count; i++ ) { + for ( j = 0; j < length_of_registered_ap; j++ ) { + for ( k = 0; k < ESSID_LEN; k++ ) { + if ( registered_ap[j].essid[k] == '\0' ) { + break; + } + if ( searched_ap[i].essid[k] != registered_ap[j].essid[k] ) { + goto no_match; + } + } + if ( memcmp( registered_ap[j].bssid, searched_ap[i].bssid, MACADDR_LEN) != 0 ) { + continue; + } + registered_ap[j].power = searched_ap[i].power; + match_count++; + break; +no_match:; + } + } + + if ( match_count == 0 ) { + lcd.cls(); + lcd.printf("location lost\nAP count %d", apinfo_count); + return; + } + + result_pos.x = result_pos.y = 0.0; + sum = 0.0; + for ( i = 0; i < length_of_registered_ap; i++ ) { + proportion = pow(10, registered_ap[i].power / 40.0f); // calc dB to linear number + sum += proportion; + result_pos.x += registered_pos[i].x * proportion; + result_pos.y += registered_pos[i].y * proportion; + } + result_pos.x /= sum; + result_pos.y /= sum; + + lcd.cls(); + lcd.printf("AP %d/%d, location x=%.1f, y=%.1f", match_count, apinfo_count, result_pos.x, result_pos.y); +} \ No newline at end of file