Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / server / colonet_wireless.cpp @ 518

History | View | Annotate | Download (7.5 KB)

1
/** @file colonet_wireless.c
2
 *
3
 * @brief Implementation of server-side colonet wireless library
4
 *
5
 * @author Eugene Marinelli
6
 */
7

    
8
/********************************* Includes **********************************/
9
#include <string>
10
#include <pthread.h>
11
#include <iostream>
12
#include <fstream>
13
#include <unistd.h>
14
#include <fcntl.h>
15
#include <signal.h>
16

    
17
#include <ColonetServer.h>
18
#include <wireless.h> // Colonet wireless library.
19
#include <wl_token_ring.h>
20

    
21
#include <colonet_defs.h>
22
#include <colonet_wireless.h>
23

    
24
/******************************* Definitions *********************************/
25

    
26
//Enable debug printouts
27
#define DEBUG 1
28

    
29
#ifdef DEBUG
30
#define dbg_printf(...) printf(__VA_ARGS__)
31
#define dbg_fprintf(...) fprintf(__VA_ARGS__)
32
#else
33
#define dbg_printf(...)
34
#define dbg_fprintf(...)
35
#endif
36

    
37
extern ColonetServer colonet_server;
38

    
39
static bool logging_enabled;
40
static char log_filename[80];
41
static pthread_t listener_thread;
42
static char wl_port[40];
43

    
44
/************************* Internal prototypes *******************************/
45
static void* listen(void* args);
46
static void timeout_handler(void);
47
static void handle_response(int frame, int received);
48
static void handle_receive(char type, int source, unsigned char* packet, int len);
49
static void unregister(void);
50
static int log_packet(unsigned char* packet, int len);
51

    
52
/**************************** Public functions *******************************/
53
int colonet_wl_init(char* wl_port_, char* log_filename_) {
54
  if (log_filename_ != NULL) {
55
    logging_enabled = true;
56
    strcpy(log_filename, log_filename_);
57
  }
58

    
59
  strncpy(wl_port, wl_port_, 40);
60

    
61
  wl_set_com_port(wl_port);
62

    
63
  printf("Calling wl_init(%s)...\n", wl_port);
64
  if (wl_init() != 0) {
65
    fprintf(stderr, "wl_init failed.\n");
66
    return -1;
67
  }
68

    
69
  wl_token_ring_register();
70

    
71
  printf("Joining token ring...\n");
72
  if (wl_token_ring_join() != 0) {
73
    fprintf(stderr, "Failed to join token ring.\n");
74
    return -1;
75
  }
76
  printf("Joined token ring.\n");
77

    
78
  return 0;
79
}
80

    
81
void colonet_wl_kill_listener_thread() {
82
  pthread_kill(listener_thread, 9);
83
}
84

    
85
int colonet_wl_run_listener_thread() {
86
  dbg_printf("Spawning listener thread...\n");
87

    
88
  if (pthread_create(&listener_thread, NULL, listen, NULL)) {
89
    perror("pthread_create");
90
    return -1;
91
  }
92

    
93
  return 0;
94
}
95

    
96
int colonet_wl_send(short client_source, short dest, ColonetRobotMessageType msg_type, unsigned char msg_code,
97
  unsigned char* args) {
98
  //printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
99

    
100
  ColonetRobotServerPacket pkt;
101
  pkt.client_id = client_source;
102
  pkt.msg_code = msg_code;
103

    
104
  for (int i = 0; i < PACKET_DATA_LEN; i++) {
105
    pkt.data[i] = args[i];
106
  }
107

    
108
  if (dest == GLOBAL_DEST) {
109
    //printf("sending to global dest\n");
110
    if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
111
                              sizeof(ColonetRobotServerPacket), 0) != 0) {
112
      return -1;
113
    }
114
  } else {
115
    //("sending to specific robot: %d.\n", dest);
116
    if (wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
117
      sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID) != 0) {
118
      return -1;
119
    }
120
  }
121

    
122
  return 0;
123
}
124

    
125
int colonet_get_num_robots(void) {
126
  return wl_token_get_num_robots();
127
}
128

    
129
int* colonet_get_xbee_ids(int* numrobots) {
130
  int num_robots = wl_token_get_num_robots();
131
  int* ids = (int*)malloc(num_robots * sizeof(int));
132

    
133
  wl_token_iterator_begin();
134

    
135
  int i = 0;
136
  while (wl_token_iterator_has_next()) {
137
    ids[i] = wl_token_iterator_next();
138
    i++;
139
  }
140

    
141
  *numrobots = num_robots;
142
  return ids;
143
}
144

    
145
// Returns int**; should be freed
146
int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {
147
  int num_robots;
148
  int* ids = colonet_get_xbee_ids(&num_robots);
149

    
150
  int** m = (int**)malloc(num_robots * sizeof(int*));
151
  for (int i = 0; i < num_robots; i++) {
152
    m[i] = (int*)malloc(num_robots * sizeof(int*));
153
  }
154

    
155
  for (int i = 0; i < num_robots; i++) {
156
    for (int j = 0; j < num_robots; j++) {
157
      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);
158
    }
159
  }
160

    
161
  *numrobots = num_robots;
162
  *ids_ = ids;
163
  return m;
164
}
165

    
166
/**************************** Private functions ******************************/
167

    
168
static void timeout_handler() {
169
  // printf("colonet wireless - timeout!\n");
170
}
171

    
172
static void handle_response(int frame, int received) {
173
  //TODO: These are just here to get rid of compiler warnings
174
  frame = frame;
175
  received = received;
176
  //printf("got response.\n");
177
}
178

    
179
static void handle_receive(char type, int source, unsigned char* data, int len) {
180
  //TODO: These are just here to get rid of compiler warnings
181
  type = type;
182

    
183
  //printf("!!!***Got packet from robot***!!!\n");
184

    
185
  ColonetRobotServerPacket* pkt = (ColonetRobotServerPacket*)data;
186

    
187
  if (logging_enabled) {
188
    log_packet(data, len);
189
  }
190

    
191
  if (pkt->msg_code == ROBOT_REQUEST_POSITION_FROM_SERVER) {
192
    /* Robot has requested its position. */
193
    int robot_x, robot_y;
194
    PositionMonitor* pm = colonet_server.getPositionMonitor();
195
    int num_robots = pm->getNumVisibleRobots();
196
    int ret = pm->getRobotPosition(source, &robot_x, &robot_y);
197
    if (ret != 0 && num_robots != 1) {
198
      fprintf(stderr, "Robot %d requested position, but its position is not known.\n", source);
199
    } else {
200
      if (ret != 0 /* && num_robots == 1 */) {
201
        VisionPosition pos = pm->getFirstPosition();
202
        robot_x = pos.x;
203
        robot_y = pos.y;
204
      }
205

    
206
      unsigned char response[80];
207
      response[0] = (robot_x >> 8) & 0xFF;
208
      response[1] = robot_x & 0xFF;
209
      response[2] = (robot_y >> 8) & 0xFF;
210
      response[3] = robot_y & 0xFF;
211

    
212
      fprintf(stderr, "Robot %d requested position.  Its position is %d,%d.\n", source, robot_x, robot_y);
213

    
214
      if (colonet_wl_send(-1, source, COLONET_COMMAND, SERVER_REPORT_POSITION_TO_ROBOT, response) != 0) {
215
        fprintf(stderr, "colonet_wl_send failed!\n");
216
        exit(1);
217
      }
218
    }
219
  } else {
220
    char processed_data[80];
221
    sprintf(processed_data, "%d %d %d %s\n", RESPONSE_TO_CLIENT_REQUEST, pkt->msg_code, source, pkt->data);
222

    
223
    colonet_server.process_received_wireless_message(pkt->client_id, processed_data, strlen(processed_data));
224
  }
225
}
226

    
227
static void unregister(void) {
228
  printf("unregister\n");
229
}
230

    
231
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
232
 * Listens for bytes on the wireless port and constructs packets based on them.
233
 * Not part of the ColonetWireless class since a function pointer must be
234
 * passed in starting the thread that runs it (tricky or impossible if it's
235
 * a class function).
236
 *
237
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
238
 * @return NULL
239
 */
240
static void* listen(void* args) {
241
  //TODO: These are just here to get rid of compiler warnings
242
  args = args;
243

    
244
  printf("Called listen.\n");
245

    
246
  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,
247
                            timeout_handler,
248
                            handle_response,
249
                            handle_receive,
250
                            unregister};
251
  wl_register_packet_group(&pgh);
252

    
253
  while (1) {
254
    wl_do();
255
    usleep(1000);
256
  }
257

    
258
  wl_terminate();
259
  return NULL;
260
}
261

    
262
/** @brief
263
 *
264
 * @param pkt Packet to be logged
265
 *
266
 * @return 0 on success, -1 on failure
267
 */
268
int log_packet(unsigned char* packet, int len) {
269
  FILE* logfile = fopen(log_filename, "a");
270

    
271
  if (logfile == NULL) {
272
    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
273
    return -1;
274
  }
275

    
276
  for (int i = 0; i < len; i++) {
277
    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
278
  }
279
  fprintf(logfile, "\n");
280

    
281
  fclose(logfile);
282
  return 0;
283
}