main.c

#include <stdio.h>
#include <unistd.h>
#include "hw_impl_unix.h"
#include "quad_app.h"
#include <fcntl.h>

int handle_io_output(const char *name);
int handle_io_input(const char *name, const char *value_str);
void set_shm(float value, float *dest, pthread_mutex_t *lock);
void usage(char *executable_name);

struct VirtQuadIO *virt_quad_io;
int virt_quad_io_file_descriptor;

/**
 * Implement each of the hardware interfaces.
 */
int setup_hardware(hardware_t *hardware) {
  hardware->i2c = create_unix_i2c();
  hardware->rc_receiver = create_unix_rc_receiver();
  hardware->motors = create_unix_motors();
  hardware->uart = create_unix_uart();
  hardware->global_timer = create_unix_global_timer();
  hardware->axi_timer = create_unix_axi_timer();
  hardware->mio7_led = create_unix_mio7_led();
  hardware->sys = create_unix_system();
  return 0;
}

int main(int argc, char *argv[]) {
  int fd, opt;
  while ((opt = getopt(argc, argv, "qh")) != -1) {
    switch (opt) {
    case 'q':
      fd = open("/dev/null", O_WRONLY);
      close(STDOUT_FILENO);
      dup2(STDOUT_FILENO, fd);
      break;
    case 'h':
    default: /* '?' */
      puts("from flags");
      usage(argv[0]);
      exit(EXIT_FAILURE);
    }
  }
  if (argv[1] != NULL && strcmp(argv[1], "--help") == 0) {
    puts("from if");
    usage(argv[0]);
    exit(EXIT_FAILURE);
  }
  argc -= optind;
  argv += optind;


  // Decide if we are launching the quad or responding to a request
  // about a running quad
  if (argv[0] == NULL) {
    // Prepare the shared memory for io. Clear memory and initialize.
    puts("Setting up...");
    int *fd = &virt_quad_io_file_descriptor;
    *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR | O_TRUNC, 0666);
    ftruncate(*fd, sizeof(struct VirtQuadIO));
    virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
    pthread_mutexattr_t attr;
    pthread_mutexattr_init(&attr);
    pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
    pthread_mutex_init(&virt_quad_io->led_lock, &attr);
    pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
    pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
    puts("Finished setting up.");
  }
  else {
    // Open exising shared memory for io. DO NOT CLEAR.
    int *fd = &virt_quad_io_file_descriptor;
    *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR, 0666);
    virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);

    // Meet requests
    if (strcmp(argv[0], "get") == 0 && argv[1] != NULL) {
      handle_io_output(argv[1]);
    }
    else if (strcmp(argv[0], "set") == 0 && argv[1] != NULL && argv[2] != NULL) {
      handle_io_input(argv[1], argv[2]);
    }
    return 0;
  }

  puts("Starting the quad application");
  quad_main(setup_hardware);
  return 0;
}

/**
 * The user wants to read an output by name. Get the output
 * and print to stdout.
 */
int handle_io_output(const char *name) {
  int ret = 0;
  struct VirtQuadIO *io = virt_quad_io;
  if (strcmp(name, "led") == 0) {
    pthread_mutex_lock(&io->led_lock);
    printf("%d\n", io->led);
    pthread_mutex_unlock(&io->led_lock);
  }
  else if (strcmp(name, "motors") == 0) {
    pthread_mutex_lock(&io->motors_lock);
    int i;
    for (i = 0; i < 4; i += 1) {
      printf("%f\n", io->motors[i]);
    }
    pthread_mutex_unlock(&io->motors_lock);
  }
  else {
    puts("Given output not recognized.");
    ret = -1;
  }
  return ret;
}

/**
 * The user wants to set an input by name, and has supplied
 * a value. Set the appropriate input to the given value.
 */
int handle_io_input(const char *name, const char *value_str) {
  int ret = 0;
  errno = 0;
  float value = strtof(value_str, NULL);
  if (errno) {
    ret = -1;
    puts("Given number format was bad.");
  }
  else {
    struct VirtQuadIO *io = virt_quad_io;
    if (strcmp(name, "rc_throttle") == 0) {
      set_shm(value, &io->rc_receiver[THROTTLE], &io->rc_lock);
    }
    else if (strcmp(name, "rc_roll") == 0) {
      set_shm(value, &io->rc_receiver[ROLL], &io->rc_lock);
    }
    else if (strcmp(name, "rc_pitch") == 0) {
      set_shm(value, &io->rc_receiver[PITCH], &io->rc_lock);
    }
    else if (strcmp(name, "rc_yaw") == 0) {
      set_shm(value, &io->rc_receiver[YAW], &io->rc_lock);
    }
    else if (strcmp(name, "rc_gear") == 0) {
      set_shm(value, &io->rc_receiver[GEAR], &io->rc_lock);
    }
    else if (strcmp(name, "rc_flap") == 0) {
      set_shm(value, &io->rc_receiver[FLAP], &io->rc_lock);
    }
    else {
      puts("Given input not recognized.");
      ret = -1;
    }
  }
  return ret;
}

void set_shm(float value, float *dest, pthread_mutex_t *lock) {
  pthread_mutex_lock(lock);
  *dest = value;
  pthread_mutex_unlock(lock);
}

void usage(char *executable_name) {
  printf("Usage: %s [ -h | --help | -q ] [ command ]\n", executable_name);
  puts("Overview:");
  puts("  The virtual quad emulates the behavior of the real quad in the Unix");
  puts("  environment. Start the virtual quad in one tab (no arguments), and");
  puts("  then control the I/O of the quad through commands.");
  puts("Commands:");
  puts("  get output");
  puts("  set input value");
  puts("Outpus:");
  puts("  led");
  puts("  motors");
  puts("Inputs:");
  puts("  rc_gear");
  puts("  rc_flap");
  puts("  rc_throttle");
  puts("  rc_pitch");
  puts("  rc_roll");
  puts("  rc_yaw");
  puts("Examples:");
  printf("  %s get led    # prints 0 or 1 to stdout\n", executable_name);
  printf("  in%s set rc_gear 1    # sets gear to \"on\" \n", executable_name);
}