Colony

/** @file colonet_wireless.c

 *

 * @brief Implementation of server-side colonet wireless library

 *

 * @author Eugene Marinelli

 */

/********************************* Includes **********************************/

#include <string>

#include <pthread.h>

#include <iostream>

#include <fstream>

#include <unistd.h>

#include <fcntl.h>

#include <signal.h>

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

#include <wl_token_ring.h>

#include <colonet_defs.h>

#include <colonet_wireless.h>

/******************************* Definitions *********************************/

//Enable debug printouts

#define DEBUG 1

#ifdef DEBUG

#define dbg_printf(...) printf(__VA_ARGS__)

#define dbg_fprintf(...) fprintf(__VA_ARGS__)

#else

#define dbg_printf(...)

#define dbg_fprintf(...)

#endif

static MsgHandlerFunction message_handler;

static bool logging_enabled;

static char log_filename[80];

static pthread_t listener_thread;

static char wl_port[40];

/************************* Internal prototypes *******************************/

static void* listen(void* args);

static void timeout_handler(void);

static void handle_response(int frame, int received);

static void handle_receive(char type, int source, unsigned char* packet, int len);

static void unregister(void);

static int log_packet(unsigned char* packet, int len);

/**************************** Public functions *******************************/

int colonet_wl_init(char* wl_port_, MsgHandlerFunction message_handler_, char* log_filename_) {

  if (log_filename_ != NULL) {

    logging_enabled = true;

    strcpy(log_filename, log_filename_);

  }

  strncpy(wl_port, wl_port_, 40);

  message_handler = message_handler_;

  wl_set_com_port(wl_port);

  printf("Calling wl_init(%s)...\n", wl_port);

  if (wl_init() != 0) {

    fprintf(stderr, "wl_init failed.\n");

    return -1;

  }

  wl_token_ring_register();

  printf("Joining token ring...\n");

  if (wl_token_ring_join() != 0) {

    fprintf(stderr, "Failed to join token ring.\n");

    return -1;

  }

  printf("Joined token ring.\n");

  return 0;

}

void colonet_wl_kill_listener_thread() {

  pthread_kill(listener_thread, 9);

}

int colonet_wl_run_listener_thread() {

  dbg_printf("Spawning listener thread...\n");

  if (pthread_create(&listener_thread, NULL, listen, NULL)) {

    perror("pthread_create");

    return -1;

  }

  return 0;

}

int colonet_wl_send(short client_source, short dest, ColonetMessageType msg_type, unsigned char msg_code,

                     unsigned char* args) {

  printf("colonet_wl_send: client_source:%d, dest:%d, msg_code:%d\n", client_source, dest, msg_code);

  ColonetRobotServerPacket pkt;

  pkt.client_id = client_source;

  pkt.msg_code = msg_code;

  for (int i = 0; i < PACKET_DATA_LEN; i++) {

    pkt.data[i] = args[i];

  }

  if (dest == GLOBAL_DEST) {

    printf("sending to global dest\n");

    if (wl_send_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

                              sizeof(ColonetRobotServerPacket), 0) != 0) {

      return -1;

    }

  } else {

    printf("sending to specific robot.\n");

    wl_send_robot_to_robot_global_packet(COLONET_PACKET_GROUP_ID, (char)msg_type, (char*)(&pkt),

                                         sizeof(ColonetRobotServerPacket), dest, COLONET_RESPONSE_PACKET_FRAME_ID);

  }

}

int colonet_get_num_robots(void) {

  return wl_token_get_num_robots();

}

int* colonet_get_xbee_ids(int* numrobots) {

  int num_robots = wl_token_get_num_robots();

  int* ids = (int*)malloc(num_robots * sizeof(int));

  wl_token_iterator_begin();

  int* p = ids;

  while (wl_token_iterator_has_next()) {

    *p = wl_token_iterator_next();

    p++;

  }

  *numrobots = num_robots;

  return ids;

}

// Returns int**; should be freed

int** colonet_get_sensor_matrix(int* numrobots, int** ids_) {

  int num_robots;

  int* ids = colonet_get_xbee_ids(&num_robots);

  int** m = (int**)malloc(num_robots * sizeof(int*));

  for (int i = 0; i < num_robots; i++) {

    m[i] = (int*)malloc(num_robots * sizeof(int*));

  }

  for (int i = 0; i < num_robots; i++) {

    for (int j = 0; j < num_robots; j++) {

      m[i][j] = wl_token_get_sensor_reading(ids[i], ids[j]);

    }

  }

  *numrobots = num_robots;

  *ids_ = ids;

  return m;

}

/**************************** Private functions ******************************/

static void timeout_handler() {

  printf("colonet wireless - timeout!\n");

}

static void handle_response(int frame, int received) {

  printf("handle response\n");

}

static void handle_receive(char type, int source, unsigned char* packet,

                           int len) {

  ColonetRobotServerPacket pkt;

  pkt.client_id = packet[0] | (packet[1]<<8) | (packet[2]<<16) | (packet[3]<<24); //little endian

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

  if (logging_enabled) {

    log_packet(packet, len);

  }

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

  printf("handle receive\n");

}

static void unregister(void) {

  printf("unregister\n");

}

/** @brief Analogous to the "SIGNAL" or "ISR" function on the robots.

 * Listens for bytes on the wireless port and constructs packets based on them.

 * Not part of the ColonetWireless class since a function pointer must be

 * passed in starting the thread that runs it (tricky or impossible if it's

 * a class function).

 *

 * @param args Pointer to arguments.  Can be safely casted to ListenerArgs type

 * @return NULL

 */

static void* listen(void* args) {

  printf("Called listen.\n");

  PacketGroupHandler pgh = {COLONET_PACKET_GROUP_ID,

                            timeout_handler,

                            handle_response,

                            handle_receive,

                            unregister};

  wl_register_packet_group(&pgh);

  while (1) {

    wl_do();

    usleep(50000);

  }

  wl_terminate();

  pthread_exit(NULL);

  return NULL;

}

/** @brief

 *

 * @param pkt Packet to be logged

 *

 * @return 0 on success, -1 on failure

 */

int log_packet(unsigned char* packet, int len) {

  FILE* logfile = fopen(log_filename, "a");

  if (logfile == NULL) {

    dbg_printf("%s: Error - fopen %s failed.\n", log_filename, __FUNCTION__);

    return -1;

  }

  for (int i = 0; i < len; i++) {

    fprintf(logfile, "%d ", *((unsigned char*)packet + i));

  }

  fprintf(logfile, "\n");

  fclose(logfile);

  return 0;

}