hw_impl_unix_pwm_output.c

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

void * output_caches();

static char *output_pwms[4];
static unsigned long cache[4];
pthread_t worker;

int unix_pwm_output_reset(struct PWMOutputDriver *self) {
  output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
  output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor2";
  output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor3";
  output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor4";

  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
  int i;
  for (i = 0; i < 4; i += 1) {
    unlink(output_pwms[i]);
    mkfifo(output_pwms[i], 0666);
  }

  // Start up worker thread whose job is to update the caches
  pthread_create(&worker, 0, output_caches, NULL);

  return 0;
}

int unix_pwm_output_write(struct PWMOutputDriver *self,
                          unsigned int channel,
                          unsigned long pulse_width_us) {
  if (cache[channel] != pulse_width_us) {
    printf("%s: %ld\n", output_pwms[channel], pulse_width_us);
  }
  cache[channel] = pulse_width_us;
  return 0;
}

void * output_caches() {
  char buff[16];
  int i;
  while (1) {
    usleep(500); // don't spam the reader
    for (i = 0; i < 4; i += 1) {
      int fifo = open(output_pwms[i], O_WRONLY | O_NONBLOCK);
      if (fifo >= 0) {
        sprintf(buff, " %ld ", cache[i]);
        write(fifo, buff, strlen(buff));
      }
      close(fifo);
    }
  }
  return NULL;
}