Statistics
| Revision:

root / branches / colonetmk2 / code / projects / swarm / colonet_wireless.cpp @ 1456

History | View | Annotate | Download (4.54 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 <stdio.h>
10
#include <string.h>
11
#include <stdlib.h>
12
#include <unistd.h>
13
#include <pthread.h>
14
#include <signal.h>
15

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

    
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 pthread_t listener_thread;
35
static char wl_port[40];
36

    
37
/************************* Internal prototypes *******************************/
38
static void* listen(void* args);
39

    
40
/**************************** Public functions *******************************/
41
/**
42
 * @brief Initializes the wireless library
43
 *
44
 * @param wl_port_ The port to listen for wireless messages on
45
 *
46
 * @return 0 on success, negative error code on failure
47
 */
48
int colonet_wl_init(char* wl_port_) {
49
  strncpy(wl_port, wl_port_, 40);
50

    
51
  wl_set_com_port(wl_port);
52

    
53
  fprintf(stderr, "Calling wl_init(%s)...\n", wl_port);
54
  if (wl_init() != 0) {
55
    fprintf(stderr, "wl_init failed.\n");
56
    return -1;
57
  }
58

    
59
  fprintf(stderr, "Setting channel\n");
60
  wl_set_channel(0xF);
61
  wl_set_pan(0x3332);
62

    
63
  /*wl_token_ring_register();
64

65
  fprintf(stderr, "Joining token ring...\n");
66
  if (wl_token_ring_join() != 0) {
67
    fprintf(stderr, "Failed to join token ring.\n");
68
    return -1;
69
  }
70
  fprintf(stderr, "Joined token ring.\n");*/
71

    
72
  return 0;
73
}
74

    
75
/**
76
 * @brief This function will kill the thread that is listening for wireless messages
77
 */
78
void colonet_wl_kill_listener_thread() {
79
  pthread_kill(listener_thread, 9);
80
}
81

    
82
void colonet_wl_join() {
83
  pthread_join(listener_thread, NULL);
84
}
85

    
86
/**
87
 * @brief This function spawns a thread to listen for wireless messages
88
 *
89
 * @return 0 on success, negative error code on failure
90
 */
91
int colonet_wl_run_listener_thread() {
92
  dbg_printf("Spawning listener thread...\n");
93

    
94
  if (pthread_create(&listener_thread, NULL, listen, NULL)) {
95
    perror("pthread_create");
96
    return -1;
97
  }
98

    
99
  return 0;
100
}
101

    
102
/**
103
 * @brief Used to get the number of robots in the token ring
104
 *
105
 * @return The result of wl_token_get_num_robots()
106
 */
107
int colonet_get_num_robots(void) {
108
  return wl_token_get_num_robots();
109
}
110

    
111
/**
112
 * @brief Gets a list of the xbee ids of the ones in the token ring
113
 *
114
 * @param numrobots A pointer to the variable where you want the number of robots to be stored
115
 *
116
 * @return A dynamically allocated piece of memory that contains the list of xbee ids.
117
 *   The function calling this function must free this memory when done with it.
118
 */
119
void colonet_get_xbee_ids(int ids[]) {
120
  wl_token_iterator_begin();
121

    
122
  int i = 0;
123
  while (wl_token_iterator_has_next()) {
124
    ids[i++] = wl_token_iterator_next();
125
  }
126
}
127

    
128
/**
129
 * @brief Gets the sensor matrix from the wireless library
130
 *
131
 * @param numrobots A pointer to the variable where you want the number of robots to be stored
132
 * @param ids_ A pointer to a pointer that you want to point to the list of xbee ids.  This memory is dynamically
133
 * allocated and should be freed by the calling process when it is done with it
134
 *
135
 * @return The 2d matrix reprsenting the sensor matrix.  This memory is dynamically allocated and should be freed by
136
 * the calling process when it is done with it
137
 */
138
void colonet_get_sensor_matrix(int* ids, SensorReading*** m) {
139
  int num_robots = colonet_get_num_robots();
140
  colonet_get_xbee_ids(ids);
141

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

    
149
/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.
150
 * Listens for bytes on the wireless port and constructs packets based on them.
151
 * Not part of the ColonetWireless class since a function pointer must be
152
 * passed in starting the thread that runs it (tricky or impossible if it's
153
 * a class function).
154
 *
155
 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type
156
 * @return NULL
157
 */
158
static void* listen(void* args) {
159
  args = args; //TODO: These are just here to get rid of compiler warnings.
160

    
161
  fprintf(stderr, "Called listen.\n");
162

    
163
  while (1) {
164
    wl_do();
165

    
166
    //this sleep is here so the thread this runs in doesn't hog the cpu
167
    pthread_yield();
168
  }
169

    
170
  fprintf(stderr, "Listen is returning.\n");
171

    
172
  wl_terminate();
173
  return NULL;
174
}
175