Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / lib / colonet_wireless / colonet_wireless.cpp @ 348

History | View | Annotate | Download (5.91 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 <wireless.h> // Colonet wireless library.
18
#include <wl_token_ring.h>
19

    
20
#include "colonet_defs.h"
21
#include "colonet_wireless.h"
22

    
23
/******************************* Definitions *********************************/
24

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

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

    
36
static MsgHandlerFunction message_handler;
37
static bool logging_enabled;
38
static char log_filename[80];
39
static pthread_t listener_thread;
40
static char wl_port[40];
41

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

    
50
/**************************** Public functions *******************************/
51
void colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) {
52
  if (log_filename_ != NULL) {
53
    logging_enabled = true;
54
    strcpy(log_filename, log_filename_);
55
  }
56

    
57
  strncpy(wl_port, wl_port_, 40);
58

    
59
  message_handler = message_handler_;
60

    
61
  printf("Calling wl_init...\n");
62
  wl_init();
63
  printf("wl_init succeeded.\n");
64

    
65
  wl_token_ring_register();
66

    
67
  printf("Joining token ring...\n");
68
  wl_token_ring_join();
69
  printf("Joined token ring.\n");
70
}
71

    
72
void colonet_wl_kill_listener_thread() {
73
  pthread_kill(listener_thread, 9);
74
}
75

    
76
int colonet_wl_run_listener_thread() {
77
  dbg_printf("Spawning listener thread...\n");
78

    
79
  if (pthread_create(&listener_thread, NULL, listen, NULL)) {
80
    perror("pthread_create");
81
    return -1;
82
  }
83

    
84
  return 0;
85
}
86

    
87
void colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,
88
                     unsigned char* args) {
89
  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);
90

    
91
  ColonetRobotServerPacket pkt;
92
  pkt.client_id = client_source;
93
  pkt.msg_code = msg_code;
94

    
95
  for (int i = 0; i < PACKET_DATA_LEN; i++) {
96
    pkt.data[i] = args[i];
97
  }
98

    
99
  if (dest == GLOBAL_DEST) {
100
    printf("sending to global dest\n");
101

    
102
    wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt), sizeof(ColonetRobotServerPacket), 0);
103
  } else {
104
    printf("sending to specific robot.\n");
105
    wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),
106
                                         sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID);
107
  }
108
}
109

    
110
int colonet_get_num_robots(void) {
111
  return wl_token_get_num_robots();
112
}
113

    
114
int* colonet_get_xbee_ids(int* numrobots) {
115
  int num_robots = wl_token_get_num_robots();
116
  int* ids = (int*)malloc(num_robots * sizeof(int));
117

    
118
  wl_token_iterator_begin();
119

    
120
  int* p = ids;
121
  while (wl_token_iterator_has_next()) {
122
    *p = wl_token_iterator_next();
123
    p++;
124
  }
125

    
126
  *numrobots = num_robots;
127
  return ids;
128
}
129

    
130
// Returns int**; should be freed
131
int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {
132
  int num_robots;
133
  int* ids = colonet_get_xbee_ids(&num_robots);
134

    
135
  int** m = (int**)malloc(num_robots * sizeof(int*));
136
  for (int i = 0; i < num_robots; i++) {
137
    m[i] = (int*)malloc(num_robots * sizeof(int*));
138
  }
139

    
140
  for (int i = 0; i < num_robots; i++) {
141
    for (int j = 0; j < num_robots; j++) {
142
      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);
143
    }
144
  }
145

    
146
  *numrobots = num_robots;
147
  *ids_ = ids;
148
  return m;
149
}
150

    
151
/**************************** Private functions ******************************/
152

    
153
static void timeout_handler() {
154
  printf("colonet wireless - timeout!\n");
155
}
156

    
157
static void handle_response(int frame, int received) {
158
  printf("handle response\n");
159
}
160

    
161
static void handle_receive(char type, int source, unsigned char* packet,
162
                           int len) {
163
  ColonetRobotServerPacket pkt;
164
  pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) | (packet[3]<<24); //little endian
165

    
166
  memcpy(pkt.data, packet + 5, PACKET_DATA_LEN);
167

    
168
  if (logging_enabled) {
169
    log_packet(packet, len);
170
  }
171

    
172
  message_handler(type, source, pkt.client_id, packet, len);
173

    
174
  printf("handle receive\n");
175
}
176

    
177
static void unregister(void) {
178
  printf("unregister\n");
179
}
180

    
181
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
182
 * Listens for bytes on the wireless port and constructs packets based on them.
183
 * Not part of the ColonetWireless class since a function pointer must be
184
 * passed in starting the thread that runs it (tricky or impossible if it's
185
 * a class function).
186
 *
187
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
188
 * @return NULL
189
 */
190
static void* listen(void* args) {
191
  printf("Called listen.\n");
192

    
193
  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,
194
                            timeout_handler,
195
                            handle_response,
196
                            handle_receive,
197
                            unregister};
198
  wl_register_packet_group(&pgh);
199

    
200
  while (1) {
201
    wl_do();
202
  }
203

    
204
  wl_terminate();
205
  pthread_exit(NULL);
206
  return NULL;
207
}
208

    
209
/** @brief
210
 *
211
 * @param pkt Packet to be logged
212
 *
213
 * @return 0 on success, -1 on failure
214
 */
215
int log_packet(unsigned char* packet, int len) {
216
  FILE* logfile = fopen(log_filename, "a");
217

    
218
  if (logfile == NULL) {
219
    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
220
    return -1;
221
  }
222

    
223
  for (int i = 0; i < len; i++) {
224
    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
225
  }
226
  fprintf(logfile, "\n");
227

    
228
  fclose(logfile);
229
  return 0;
230
}