From f61105587a89b7b8375f430f2a74659a49fb4d9a Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Mon, 27 Mar 2017 22:01:46 -0500
Subject: [PATCH] quad: implement PWM input and outputs for the virtual quad
---
quad/executable.mk | 4 +-
quad/src/virt_quad/Makefile | 4 +-
quad/src/virt_quad/README.md | 17 ++++
quad/src/virt_quad/hw_impl_unix_pwm_input.c | 85 +++++++++++---------
quad/src/virt_quad/hw_impl_unix_pwm_output.c | 27 ++++++-
5 files changed, 97 insertions(+), 40 deletions(-)
create mode 100644 quad/src/virt_quad/README.md
diff --git a/quad/executable.mk b/quad/executable.mk
index d17244a83..0f5f64352 100644
--- a/quad/executable.mk
+++ b/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
diff --git a/quad/src/virt_quad/Makefile b/quad/src/virt_quad/Makefile
index 3626790b0..76e2b857d 100644
--- a/quad/src/virt_quad/Makefile
+++ b/quad/src/virt_quad/Makefile
@@ -1,6 +1,8 @@
TOP=../..
-NAME = virt_quad
+NAME = virt-quad
REQLIBS = -lquad_app -lcomputation_graph -lm -lcommands -lgraph_blocks
include $(TOP)/executable.mk
+
+CLEANUP += virt-quad-fifos
diff --git a/quad/src/virt_quad/README.md b/quad/src/virt_quad/README.md
new file mode 100644
index 000000000..e7789797a
--- /dev/null
+++ b/quad/src/virt_quad/README.md
@@ -0,0 +1,17 @@
+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
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_input.c b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
index 8881f5f38..60c51234f 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_input.c
+++ b/quad/src/virt_quad/hw_impl_unix_pwm_input.c
@@ -1,50 +1,61 @@
#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;
}
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_output.c b/quad/src/virt_quad/hw_impl_unix_pwm_output.c
index b549cd436..3e094b37f 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_output.c
+++ b/quad/src/virt_quad/hw_impl_unix_pwm_output.c
@@ -1,12 +1,37 @@
#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;
}
--
GitLab