hw_impl_unix_motor.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_cache();

static char *output_pwms[4];
static float cache[4];
pthread_t worker;
static void float_equals(float a, float b);

static int zero = 0;
static int one = 1;
static int two = 2;
static int three = 3;

int unix_motor_reset(struct MotorDriver *self) {
  output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/motor1";
  output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/motor2";
  output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/motor3";
  output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/motor4";

  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);

  // Start up worker thread whose job is to update the caches
  pthread_create(&worker, 0, output_cache, &zero);
  pthread_create(&worker, 0, output_cache, &one);
  pthread_create(&worker, 0, output_cache, &two);
  pthread_create(&worker, 0, output_cache, &three);

  return 0;
}

int unix_motor_write(struct MotorDriver *self,
                          unsigned int channel,
                          float magnitude) {
  if (cache[channel] != magnitude) {
    printf("%s: %.2f\n", output_pwms[channel], magnitude);
  }
  cache[channel] = magnitude;
  return 0;
}

void * output_cache(void *arg) {
  int *output_index = arg;
  char buff[16];
  int i = *output_index;

  // Setup FIFO
  unlink(output_pwms[i]);
  mkfifo(output_pwms[i], 0666);

  // Block while waiting for someone to listen
  while (1) {
    int fifo = open(output_pwms[i], O_WRONLY);
    sprintf(buff, "%.2f\n", cache[i]);
    write(fifo, buff, strlen(buff));
    close(fifo);
    pthread_yield();
  }
  return NULL;
}

static void float_equals(float a, float b) {
  return fabs(a - b) < 0.00001;
}