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>

static char *fifo_dir = "virt-quad-fifos";
static char *pwms[6];
static int fifos[6];
static unsigned long cache[6];

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

  mkdir(fifo_dir, 0777);
  int i;
  for (i = 0; i < 6; i += 1) {
    unlink(pwms[i]);
    char fifoname[64];
    sprintf(fifoname, "%s/%s", fifo_dir, pwms[i]);
    mkfifo(fifoname, 0666);
    fifos[i] = open(fifoname, O_RDONLY | O_NONBLOCK);
  }

  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", pwms[i], cache[i]);
  }

  return 0;
}

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

  char buff[16];
  int bytes_read = read(fifos[channel], buff, 15);
  if (bytes_read >= 6) {
    buff[bytes_read] = '\0';
    unsigned long val = strtoll(buff, NULL, 10);
    if (val < max && val > min) {
      cache[channel] = val;
      printf("%s: %d\n", pwms[channel], val);
    }
  }

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