hw_impl_unix_pwm_input.c

#include "hw_impl_unix.h"
#include "controllers.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>

void * update_input_cache();

static char *input_names[6];
static int fifos[6];
static unsigned long cache[6];
static pthread_t workers[6];
static int nums[] = {0, 1, 2, 3, 4, 5};

int unix_pwm_input_reset(struct PWMInputDriver *self) {
  input_names[0] = "pwm-input-throttle";
  input_names[1] = "pwm-input-roll";
  input_names[2] = "pwm-input-pitch";
  input_names[3] = "pwm-input-yaw";
  input_names[4] = "pwm-input-gear";
  input_names[5] = "pwm-input-flap";

  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);

  // Start up worker thread whose job is to update the caches
  int i;
  for (i = 0; i < 6; i += 1) {
    pthread_create(&workers[i], 0, update_input_cache, &nums[i]);
    usleep(1000);
  }

  cache[0] = THROTTLE_MIN;
  cache[1] = ROLL_CENTER;
  cache[2] = PITCH_CENTER;
  cache[3] = YAW_CENTER;
  cache[4] = GEAR_0;
  cache[5] = FLAP_1;

  for (i = 0; i < 6; i += 1) {
    printf("%s: %d\n", input_names[i], cache[i]);
  }

  return 0;
}

int unix_pwm_input_read(struct PWMInputDriver *self,
                        unsigned int channel,
                        unsigned long *pulse_width_us) {

  *pulse_width_us = cache[channel];
  return 0;
}

void * update_input_cache(void *arg) {
  int *cache_index = arg;
  int i = *cache_index;
  char buff[16];

  // Setup FIFO
  unlink(input_names[i]);
  char fifoname[64];
  sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
  mkfifo(fifoname, 0666);
  fifos[i] = open(fifoname, O_RDONLY);

  // Block while waiting for reads
  while (1) {
    int bytes_read = read(fifos[i], buff, 15);
    if (bytes_read > 0) {
      buff[bytes_read] = '\0';
      unsigned long val = strtoll(buff, NULL, 10);
      if (val < max && val > min) {
	cache[i] = val;
	printf("%s: %d\n", input_names[i], val);
      }
      else {
	printf("%s: Bad value - input not received\n", input_names[i]);
      }
    }
  }
  return NULL;
}