quad: implement PWM input and outputs for the virtual quad

quad/executable.mk

@@ -12,6 +12,8 @@ OBJECTS = $(patsubst %.c, $(OBJDIR)/%.o, $(SOURCES))
 
 TARGET = $(EXEDIR)/$(NAME)
 
+CLEANUP = $(TARGET) $(OBJDIR)
+
 .PHONY: default run clean
 
 ################
@@ -24,7 +26,7 @@ run: $(TARGET)
 	$(EXEDIR)/$(NAME)
 
 clean:
-	rm -rf $(TARGET) $(OBJDIR)
+	rm -rf $(CLEANUP)
 
 ####################
 ## Internal Targets

quad/src/virt_quad/Makefile

 TOP=../..
 
-NAME = virt_quad
+NAME = virt-quad
 REQLIBS = -lquad_app -lcomputation_graph -lm -lcommands -lgraph_blocks
 
 include $(TOP)/executable.mk
+
+CLEANUP += virt-quad-fifos

quad/src/virt_quad/README.md

+The Virtual Quadcopter
+----
+Look at how modular our quad is ... it can even run in a Unix environment!
+
+But really, this isn't just some token project, this is pretty useful for
+debugging routine things in the quad_app without having to fire up the
+plethoria of things required for flight in Coover 3050. In fact, you don't 
+even have to be in Coover 3050...
+
+# Using the Virtual Quad
+Start it up:
+```
+make run
+```
+
+And you can do things with it. You'll notice it will make a bunch of FIFOs in
+the current directory. Write to / read from these FIFOs as you see fit.
\ No newline at end of file

quad/src/virt_quad/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) {
-  static int inc = 0;
-  unsigned long gear;
-
-  switch (channel) {
-  case 0:
-    *pulse_width_us = 100000;
-    break;
-  case 1:
-    *pulse_width_us = 100000;
-    break;
-  case 2:
-    *pulse_width_us = 100000;
-    break;
-  case 3:
-    *pulse_width_us = 100000;
-    break;
-  case 4:
-    if (inc == 0) {
-      inc += 1;
-      puts("GEAR OFF");
-    }
-    if (inc < 20) {
-      inc += 1;
-      gear = 120000;
-    }
-    else if (inc == 20) {
-      puts("GEAR ON");
-      inc += 1;
-    }
-    else {
-      gear = 140000;
+
+  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 = gear;
-    break;
-  case 5:
-    // flap 1
-    *pulse_width_us = 192000;
-    break;
   }
+
+  *pulse_width_us = cache[channel];
   return 0;
 }

quad/src/virt_quad/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>
+
+static char *fifo_dir = "virt-quad-fifos";
+static char *output_pwms[4];
 
 int unix_pwm_output_reset(struct PWMOutputDriver *self) {
+  output_pwms[0] = "virt-quad-fifos/pwm-output-motor1";
+  output_pwms[1] = "virt-quad-fifos/pwm-output-motor2";
+  output_pwms[2] = "virt-quad-fifos/pwm-output-motor3";
+  output_pwms[3] = "virt-quad-fifos/pwm-output-motor4";
+
+  mkdir("virt-quad-fifos", 0777);
+  int i;
+  for (i = 0; i < 4; i += 1) {
+    unlink(output_pwms[i]);
+    mkfifo(output_pwms[i], 0666);
+  }
+
   return 0;
 }
 
 int unix_pwm_output_write(struct PWMOutputDriver *self,
                           unsigned int channel,
                           unsigned long pulse_width_us) {
-  //printf("PWM OUTPUT: %d %d\n", channel, pulse_width_us);
+  char buff[16];
+  int fifo = open(output_pwms[channel], O_WRONLY | O_NONBLOCK);
+  if (fifo >= 0) {
+    sprintf(buff, "%d\0", pulse_width_us);
+    int bytes_read = write(fifo, buff, strlen(buff));
+  }
+  close(fifo);
   return 0;
 }