Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (4.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

    
16
#include <wireless.h> // Colonet wireless library.
17

    
18
#include "colonet_defs.h"
19
#include "colonet_wireless.h"
20

    
21
/******************************* Definitions *********************************/
22

    
23
//Enable debug printouts
24
#define DEBUG 1
25

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

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

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

    
49

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

    
58
  strncpy(wl_port, wl_port_, 40);
59

    
60
  // todo - initialize wireless port
61

    
62
  message_handler = message_handler_;
63
}
64

    
65
void colonet_wl_kill_listener_thread() {
66
  pthread_kill(listener_thread, 9);
67
}
68

    
69
int colonet_wl_run_listener_thread() {
70
  dbg_printf("Spawning listener thread...\n");
71

    
72
  if (pthread_create(&listener_thread, NULL, listen, NULL)) {
73
    perror("pthread_create");
74
    return -1;
75
  }
76

    
77
  return 0;
78
}
79

    
80
void colonet_wl_send(int client_source, short dest,
81
                     ColonetMessageType msg_type, unsigned char msg_code,
82
                     unsigned char* args) {
83
  ColonetRobotServerPacket pkt;
84
  pkt.client_id = client_source;
85
  pkt.msg_code = msg_code;
86

    
87
  for (int i = 0; i < PACKET_DATA_LEN; i++) {
88
    pkt.data[i] = args[i];
89
  }
90

    
91
  char pkt_buffer[40];
92
  memcpy(pkt_buffer, (char*)(&pkt), sizeof(ColonetRobotServerPacket));
93

    
94
  if (dest == GLOBAL_DEST) {
95
    printf("sending to global dest\n");
96
    wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, pkt_buffer,
97
                          1 + PACKET_DATA_LEN, 0);
98
  } else {
99
    printf("sending to specific robot.\n");
100
    wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type,
101
                                         pkt_buffer, 1 + PACKET_DATA_LEN, dest,
102
                                         COLONET_RESPONSE_PACKET_FRAME_ID);
103
  }
104
}
105

    
106
/**************************** Private functions ******************************/
107

    
108
static void timeout_handler() {
109
  printf("timeout!\n");
110
}
111

    
112
static void handle_response(int frame, int received) {
113
  printf("handle response\n");
114
}
115

    
116
static void handle_receive(char type, int source, unsigned char* packet,
117
                           int len) {
118
  ColonetRobotServerPacket pkt;
119
  pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) |
120
    (packet[3]<<24); //little endian
121

    
122
  memcpy(pkt.data, packet + 5, PACKET_DATA_LEN);
123

    
124
  if (logging_enabled) {
125
    log_packet(packet, len);
126
  }
127

    
128
  message_handler(type, source, pkt.client_id, packet, len);
129

    
130
  printf("handle receive\n");
131
}
132

    
133
static void unregister(void) {
134
  printf("unregister\n");
135
}
136

    
137
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
138
 * Listens for bytes on the wireless port and constructs packets based on them.
139
 * Not part of the ColonetWireless class since a function pointer must be
140
 * passed in starting the thread that runs it (tricky or impossible if it's
141
 * a class function).
142
 *
143
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
144
 * @return NULL
145
 */
146
static void* listen(void* args) {
147
  printf("Called listen.\n");
148
  wl_init();
149
  printf("wl_init succeeded.\n");
150
  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,
151
                            timeout_handler,
152
                            handle_response,
153
                            handle_receive,
154
                            unregister};
155
  wl_register_packet_group(&pgh);
156

    
157
  while (1) {
158
    wl_do();
159
  }
160

    
161
  wl_terminate();
162
  pthread_exit(NULL);
163
  return NULL;
164
}
165

    
166
/** @brief
167
 *
168
 * @param pkt Packet to be logged
169
 *
170
 * @return 0 on success, -1 on failure
171
 */
172
int log_packet(unsigned char* packet, int len)
173
{
174
  FILE* logfile = fopen(log_filename, "a");
175

    
176
  if (logfile == NULL) {
177
    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);
178
    return -1;
179
  }
180

    
181
  for (int i = 0; i < len; i++) {
182
    fprintf(logfile, "%d ", *((unsigned char*)packet + i));
183
  }
184
  fprintf(logfile, "\n");
185

    
186
  fclose(logfile);
187
  return 0;
188
}