Revision 297
trying to get server to send back num robots, xbee ids, sensor matrix -- not working yet
| colonet_wireless.cpp | ||
|---|---|---|
| 14 | 14 |
#include <fcntl.h> |
| 15 | 15 |
|
| 16 | 16 |
#include <wireless.h> // Colonet wireless library. |
| 17 |
#include <wl_token_ring.h> |
|
| 17 | 18 |
|
| 18 | 19 |
#include "colonet_defs.h" |
| 19 | 20 |
#include "colonet_wireless.h" |
| ... | ... | |
| 57 | 58 |
|
| 58 | 59 |
strncpy(wl_port, wl_port_, 40); |
| 59 | 60 |
|
| 60 |
// todo - initialize wireless port |
|
| 61 |
wl_token_ring_register(); |
|
| 62 |
wl_token_ring_join(); |
|
| 61 | 63 |
|
| 62 | 64 |
message_handler = message_handler_; |
| 63 | 65 |
} |
| ... | ... | |
| 77 | 79 |
return 0; |
| 78 | 80 |
} |
| 79 | 81 |
|
| 80 |
void colonet_wl_send(short client_source, short dest, |
|
| 81 |
ColonetMessageType msg_type, unsigned char msg_code, |
|
| 82 |
void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code, |
|
| 82 | 83 |
unsigned char* args) {
|
| 83 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n",
|
|
| 84 |
client_source, dest, msg_code); |
|
| 84 |
printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
|
|
| 85 | 85 |
|
| 86 | 86 |
ColonetRobotServerPacket pkt; |
| 87 | 87 |
pkt.client_id = client_source; |
| ... | ... | |
| 94 | 94 |
if (dest == GLOBAL_DEST) {
|
| 95 | 95 |
printf("sending to global dest\n");
|
| 96 | 96 |
|
| 97 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, |
|
| 98 |
(char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
|
| 97 |
wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0); |
|
| 99 | 98 |
} else {
|
| 100 | 99 |
printf("sending to specific robot.\n");
|
| 101 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, |
|
| 102 |
(char)msg_type, (char*)(&pkt), |
|
| 103 |
sizeof(ColonetRobotServerPacket), |
|
| 104 |
dest, |
|
| 105 |
COLONET_RESPONSE_PACKET_FRAME_ID); |
|
| 100 |
wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), |
|
| 101 |
sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID); |
|
| 106 | 102 |
} |
| 107 | 103 |
} |
| 108 | 104 |
|
| 105 |
int colonet_get_num_robots(void) {
|
|
| 106 |
return wl_token_get_num_robots(); |
|
| 107 |
} |
|
| 108 |
|
|
| 109 |
int* colonet_get_xbee_ids(int* n) {
|
|
| 110 |
int num_robots = wl_token_get_num_robots(); |
|
| 111 |
int* ids = (int*)malloc(num_robots * sizeof(int)); |
|
| 112 |
|
|
| 113 |
wl_token_iterator_begin(); |
|
| 114 |
|
|
| 115 |
int* p = ids; |
|
| 116 |
while (wl_token_iterator_has_next()) {
|
|
| 117 |
*p = wl_token_iterator_next(); |
|
| 118 |
p++; |
|
| 119 |
} |
|
| 120 |
|
|
| 121 |
*n = num_robots; |
|
| 122 |
return ids; |
|
| 123 |
} |
|
| 124 |
|
|
| 125 |
// Returns int**; should be freed |
|
| 126 |
int** colonet_get_sensor_matrix(int* n) {
|
|
| 127 |
int num_robots; |
|
| 128 |
int* ids = colonet_get_xbee_ids(&num_robots); |
|
| 129 |
|
|
| 130 |
int** m = (int**)malloc(num_robots * sizeof(int*)); |
|
| 131 |
for (int i = 0; i < num_robots; i++) {
|
|
| 132 |
m[i] = (int*)malloc(num_robots * sizeof(int*)); |
|
| 133 |
} |
|
| 134 |
|
|
| 135 |
for (int i = 0; i < num_robots; i++) {
|
|
| 136 |
for (int j = 0; j < num_robots; j++) {
|
|
| 137 |
m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]); |
|
| 138 |
} |
|
| 139 |
} |
|
| 140 |
|
|
| 141 |
free(ids); |
|
| 142 |
|
|
| 143 |
*n = num_robots; |
|
| 144 |
return m; |
|
| 145 |
} |
|
| 146 |
|
|
| 109 | 147 |
/**************************** Private functions ******************************/ |
| 110 | 148 |
|
| 111 | 149 |
static void timeout_handler() {
|
| 112 |
printf("timeout!\n");
|
|
| 150 |
printf("colonet wireless - timeout!\n");
|
|
| 113 | 151 |
} |
| 114 | 152 |
|
| 115 | 153 |
static void handle_response(int frame, int received) {
|
| ... | ... | |
| 172 | 210 |
* |
| 173 | 211 |
* @return 0 on success, -1 on failure |
| 174 | 212 |
*/ |
| 175 |
int log_packet(unsigned char* packet, int len) |
|
| 176 |
{
|
|
| 213 |
int log_packet(unsigned char* packet, int len) {
|
|
| 177 | 214 |
FILE* logfile = fopen(log_filename, "a"); |
| 178 | 215 |
|
| 179 | 216 |
if (logfile == NULL) {
|
Also available in: Unified diff