Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

quad/scripts/uart_comm_listen.py

-#!/usr/local/bin/python3.6
-
-import sys
-import time
-
-import serial
-
-def read_packet(ser):
-    header = ser.read(7)
-    length = int.from_bytes(header[5:7], byteorder='little')
-    data = ser.read(length)
-    checksum = ser.read()
-    return data
-
-if __name__ == '__main__':
-    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
-        i = 0
-        while True:
-            ser.reset_input_buffer()
-            time.sleep(0.05)
-            while ser.in_waiting != 0:
-                resp = read_packet(ser)
-                elapsed = int.from_bytes(resp[0:3], byteorder='little')
-                processed = int.from_bytes(resp[4:7], byteorder='little')
-                print("{} {} {}".format(i, elapsed, processed))
-                i += 1
-            ser.flush()
-

quad/scripts/uart_comm_send.py

-#!/usr/local/bin/python3.6
-
-import sys
-import time
-
-import serial
-
-def create_msg(main_type, subtype, msg_id, data):
-    msg = bytes()
-    msg += b'\xBE'
-    msg += main_type.to_bytes(1, 'little')
-    msg += subtype.to_bytes(1, 'little')
-    msg += msg_id.to_bytes(2, 'little')
-    msg += len(data).to_bytes(2, 'little')
-    msg += data
-    
-    checksum = 0
-    for b in msg:
-        checksum ^= b
-    msg += checksum.to_bytes(1, 'little')
-    return msg
-
-def create_test_packet(size=8):
-    data = bytes((i % 256 for i in range(size)))
-    return create_msg(0, 2, 0, data)
-
-if __name__ == '__main__':
-    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
-        for i in range(5):
-            ser.reset_input_buffer()
-            data = bytes.fromhex('be040002001c0002000000d80471be5732703f9d16093f8bf7a03d0586ab3d006d3a40c1')
-            #ser.write(create_test_packet(24))
-            ser.write(data)
-            #ser.flush()
-            #time.sleep(0.5)

quad/src/quad_app/callbacks.c

@@ -36,8 +36,11 @@ int cb_debug(struct modular_structs *structs, struct metadata *meta, unsigned ch
   * counts the number of packet logs.
   */
 int cb_packetlog(struct modular_structs* structs, struct metadata *meta, u8 *data, u16 length) {
+	char buf[64];
 	n_msg_received += 1;
 	total_payload_received += length;
+	int len = sprintf(buf, "Packets received: %d", n_msg_received);
+	send_data(&structs->hardware_struct.uart, PACKETLOG_ID, 0, buf, len);
 	return 0;
 }
 

quad/src/quad_app/controllers.h

@@ -102,6 +102,9 @@
 #define min 100000
 #define max 200000
 
+#define MOTOR_MIN 100000
+#define MOTOR_MAX 200000
+
 void filter_PWMs(int* mixer);
 void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
 void Aero_to_PWMS(int* PWMs, int* aero);

quad/src/quad_app/hw_iface.h

 #ifndef HW_IFACE_H
 #define HW_IFACE_H
 
+/**
+ * Hardware Interfaces
+ *
+ * These interfaces are used to accomplish separation between the application
+ * layer and the hardware layer in the program that runs on the quadcopter.
+ *
+ * NOTE:
+ * If you wound up here after following some IDE function declaration trail,
+ * you've hit the end of the application layer. Go to the location of the
+ * hardware layer appropriate for your circumstance:p
+ * ../../xsdk_worksapce/modular_quad_pid -> running quad_app on the Zybo
+ * ../virt_quad -> running quad_app in a Unix environment
+ */
+
 struct I2CDriver {
   void *state;
   int (*reset)(struct I2CDriver *self);

quad/src/quad_app/initialize_components.c

@@ -7,6 +7,7 @@
  
 #include "initialize_components.h"
 #include "communication.h"
+#include "controllers.h"
 #include "sensor.h"
 #include "iic_utils.h"
 
@@ -83,6 +84,12 @@ int init_structs(modular_structs_t *structs) {
   struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
   if (pwm_outputs->reset(pwm_outputs)) return -1;
 
+  // Set motor outputs to off
+  int i;
+  for (i = 0; i < 4; i += 1) {
+    pwm_outputs->write(pwm_outputs, i, MOTOR_MIN);
+  }
+
   // Initialize sensors
 
   //manual flight mode

quad/src/quad_app/quad_app.c

@@ -18,9 +18,6 @@
 #include "communication.h"
 #include "mio7_led.h"
 
-//#define BENCH_TEST
-//#define UART_BENCHMARK
-
 int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 {
 	// Structures to be used throughout
@@ -38,10 +35,8 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		return -1;
 	}
 
-#ifndef BENCH_TEST
 	// Loops to make sure the quad is responding correctly before starting the control loop
 	protection_loops(&structs);
-#endif
 
 	int last_kill_condition = kill_condition(&(structs.user_input_struct));
 
@@ -55,23 +50,8 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		// Process all received data
 
-#ifdef UART_BENCHMARK
-		usleep(500000);
-		u32 start_time = timer_get_count();
-#endif
 		process_received(&structs);
 
-#ifdef UART_BENCHMARK
-		u32 end_time = timer_get_count();
-		u32 duration = end_time - start_time;
-		u32 packets_processed = uart_buff_packets_processed();
-		u32 data[2];
-		data[0] = duration;
-		data[1] = packets_processed;
-		send_data(0, 0, 0, (char *) &data, 8);
-#endif
-
-#ifndef BENCH_TEST
 		// Get the user input and put it into user_input_struct
 		get_user_input(&(structs.hardware_struct), &(structs.log_struct), &(structs.user_input_struct));
 
@@ -94,7 +74,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		}
 		// update the GUI
 		update_GUI(&(structs.log_struct));
-#endif
+
 		// Processing of loop timer at the end of the control loop
 		timer_end_loop(&(structs.log_struct));
 

quad/src/quad_app/util.c

@@ -68,10 +68,10 @@ int read_flap(int flap)
  * Turns off the motors
  */
 void kill_motors(struct PWMOutputDriver *pwm_outputs) {
-  pwm_outputs->write(pwm_outputs, 0, 0);
-  pwm_outputs->write(pwm_outputs, 1, 0);
-  pwm_outputs->write(pwm_outputs, 2, 0);
-  pwm_outputs->write(pwm_outputs, 3, 0);
+  pwm_outputs->write(pwm_outputs, 0, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 1, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 2, MOTOR_MIN);
+  pwm_outputs->write(pwm_outputs, 3, MOTOR_MIN);
 }
 
 int build_int(u8 *buff) {

quad/src/virt_quad/Makefile

 TOP=../..
 
 NAME = virt-quad
-REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm
+REQLIBS = -Wl,--whole-archive -lquad_app -Wl,--no-whole-archive -lcommands -lgraph_blocks -lcomputation_graph -lm -lpthread
 
 include $(TOP)/executable.mk
 

quad/src/virt_quad/hw_impl_unix_i2c.c

 #include "hw_impl_unix.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include "iic_utils.h"
+
+void * update_i2c_input_cache(void *);
+
+union val {
+  unsigned char b[2];
+  unsigned short s;
+};
+
+static char *input_names[6];
+static int fifos[6];
+static union val cache[6];
+
+static short last_dev;
+static short last_reg;
+static short last_val;
+
+static int zero = 0;
+static int one = 1;
+static int two = 2;
+static int three = 3;
+static int four = 4;
+static int five = 5;
+
+static pthread_t worker;
 
 int unix_i2c_reset(struct I2CDriver *self) {
+  input_names[0] = "i2c-mpu-accel-x";
+  input_names[1] = "i2c-mpu-accel-y";
+  input_names[2] = "i2c-mpu-accel-z";
+  input_names[3] = "i2c-mpu-gryo-x";
+  input_names[4] = "i2c-mpu-gryo-y";
+  input_names[5] = "i2c-mpu-gyro-z";
+
+  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
+
+  // Start up worker thread whose job is to update the caches
+  pthread_create(&worker, 0, update_i2c_input_cache, &zero);
+  pthread_create(&worker, 0, update_i2c_input_cache, &one);
+  pthread_create(&worker, 0, update_i2c_input_cache, &two);
+  pthread_create(&worker, 0, update_i2c_input_cache, &three);
+  pthread_create(&worker, 0, update_i2c_input_cache, &four);
+  pthread_create(&worker, 0, update_i2c_input_cache, &five);
+
+  cache[0].s = 0;
+  cache[1].s = 0;
+  cache[2].s = 0;
+  cache[3].s = 0;
+  cache[4].s = 0;
+  cache[5].s = 0;
+
   return 0;
 }
 
@@ -8,6 +61,11 @@ int unix_i2c_write(struct I2CDriver *self,
                    unsigned short device_addr,
                    unsigned char *data,
                    unsigned int length) {
+  if (length == 2) {
+    last_dev = device_addr;
+    last_reg = data[0];
+    last_val = data[1];
+  }
   return 0;
 }
 
@@ -15,5 +73,53 @@ int unix_i2c_read(struct I2CDriver *self,
                   unsigned short device_addr,
                   unsigned char *buff,
                   unsigned int length) {
+  if (last_dev != device_addr) {
+    return -1;
+  }
+
+  switch (device_addr) {
+  case MPU9150_DEVICE_ADDR:
+    if (last_reg == ACCEL_GYRO_BASE_ADDR) {
+      buff[0] = cache[0].b[0];
+      buff[1] = cache[0].b[1];
+      buff[2] = cache[1].b[0];
+      buff[3] = cache[1].b[1];
+      buff[4] = cache[2].b[0];
+      buff[5] = cache[2].b[1];
+      buff[6] = 0;
+      buff[7] = 0;
+      buff[8] = cache[3].b[0];
+      buff[9] = cache[3].b[1];
+      buff[10] = cache[4].b[0];
+      buff[11] = cache[4].b[1];
+      buff[12] = cache[5].b[0];
+      buff[13] = cache[5].b[1];
+    }
+  }
   return 0;
 }
+
+void * update_i2c_input_cache(void *arg) {
+  int *cache_index = arg;
+  int i = *cache_index;
+  char buff[16];
+
+  // Setup FIFO
+  unlink(input_names[i]);
+  char fifoname[64];
+  sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
+  mkfifo(fifoname, 0666);
+  fifos[i] = open(fifoname, O_RDONLY);
+
+  // Block while waiting for reads
+  while (1) {
+    int bytes_read = read(fifos[i], buff, 15);
+    if (bytes_read > 0) {
+      buff[bytes_read] = '\0';
+      unsigned long val = strtoll(buff, NULL, 10);
+      cache[i].s = val;
+      printf("%s: %ld\n", input_names[i], val);
+    }
+  }
+  return NULL;
+}

quad/src/virt_quad/hw_impl_unix_mio7_led.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>
+#include <pthread.h>
+
+void * output_cached_led();
 
 int on;
+static char *led_fifo_name;
+pthread_t worker;
 
 int unix_mio7_led_reset(struct LEDDriver *self) {
+  led_fifo_name = VIRT_QUAD_FIFOS_DIR "/mio7-led";
+
+  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
+  int i;
+  for (i = 0; i < 4; i += 1) {
+    unlink(led_fifo_name);
+    mkfifo(led_fifo_name, 0666);
+  }
+
+  // Start up worker thread whose job is to update the caches
+  pthread_create(&worker, 0, output_cached_led, NULL);
+
   return 0;
 }
 
@@ -21,3 +44,14 @@ int unix_mio7_led_turn_off(struct LEDDriver *self) {
   }
   return 0;
 }
+
+void * output_cached_led() {
+  char buff[16];
+  while (1) {
+    int fifo = open(led_fifo_name, O_WRONLY);
+    sprintf(buff, "%d\n", on);
+    write(fifo, buff, strlen(buff));
+    close(fifo);
+    usleep(500); // don't spam the reader
+  }
+}

quad/src/virt_quad/hw_impl_unix_pwm_input.c

@@ -3,27 +3,29 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
+#include <pthread.h>
 
-static char *pwms[6];
+void * update_input_cache();
+
+static char *input_names[6];
 static int fifos[6];
 static unsigned long cache[6];
+pthread_t worker;
 
 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";
+  input_names[0] = "pwm-input-throttle";
+  input_names[1] = "pwm-input-roll";
+  input_names[2] = "pwm-input-pitch";
+  input_names[3] = "pwm-input-yaw";
+  input_names[4] = "pwm-input-gear";
+  input_names[5] = "pwm-input-flap";
 
   mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
+
+  // Start up worker thread whose job is to update the caches
   int i;
   for (i = 0; i < 6; i += 1) {
-    unlink(pwms[i]);
-    char fifoname[64];
-    sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, pwms[i]);
-    mkfifo(fifoname, 0666);
-    fifos[i] = open(fifoname, O_RDONLY | O_NONBLOCK);
+    pthread_create(&worker, 0, update_input_cache, &i);
   }
 
   cache[0] = THROTTLE_MIN;
@@ -34,7 +36,7 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
   cache[5] = FLAP_1;
 
   for (i = 0; i < 6; i += 1) {
-    printf("%s: %d\n", pwms[i], cache[i]);
+    printf("%s: %d\n", input_names[i], cache[i]);
   }
 
   return 0;
@@ -44,17 +46,36 @@ int unix_pwm_input_read(struct PWMInputDriver *self,
                         unsigned int channel,
                         unsigned long *pulse_width_us) {
 
+  *pulse_width_us = cache[channel];
+  return 0;
+}
+
+void * update_input_cache(void *arg) {
+  int *cache_index = arg;
+  int i = *cache_index;
   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);
+
+  // Setup FIFO
+  unlink(input_names[i]);
+  char fifoname[64];
+  sprintf(fifoname, "%s/%s", VIRT_QUAD_FIFOS_DIR, input_names[i]);
+  mkfifo(fifoname, 0666);
+  fifos[i] = open(fifoname, O_RDONLY);
+
+  // Block while waiting for reads
+  while (1) {
+    int bytes_read = read(fifos[i], buff, 15);
+    if (bytes_read > 0) {
+      buff[bytes_read] = '\0';
+      unsigned long val = strtoll(buff, NULL, 10);
+      if (val < max && val > min) {
+	cache[i] = val;
+	printf("%s: %d\n", input_names[i], val);
+      }
+      else {
+	printf("%s: Bad value - input not received\n", input_names[i]);
+      }
     }
   }
-
-  *pulse_width_us = cache[channel];
-  return 0;
+  return NULL;
 }

quad/src/virt_quad/hw_impl_unix_pwm_output.c

@@ -3,8 +3,19 @@
 #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 unsigned long cache[4];
+pthread_t worker;
+
+static int zero = 0;
+static int one = 1;
+static int two = 2;
+static int three = 3;
 
 int unix_pwm_output_reset(struct PWMOutputDriver *self) {
   output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
@@ -13,11 +24,12 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
   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_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;
 }
@@ -25,12 +37,29 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
 int unix_pwm_output_write(struct PWMOutputDriver *self,
                           unsigned int channel,
                           unsigned long 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));
+  if (cache[channel] != pulse_width_us) {
+    printf("%s: %ld\n", output_pwms[channel], pulse_width_us);
   }
-  close(fifo);
+  cache[channel] = pulse_width_us;
   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, "%ld\n", cache[i]);
+    write(fifo, buff, strlen(buff));
+    close(fifo);
+    usleep(500); // don't spam the reader
+  }
+  return NULL;
+}

quad/src/virt_quad/hw_impl_unix_uart.c

 #include "hw_impl_unix.h"
-#include <sys/types.h>       
-#include <sys/socket.h>
 #include <stdio.h>
-#include <sys/un.h>
-#include <sys/ioctl.h>
-#include <err.h>
-#include <netinet/in.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <fcntl.h>
 
-#define DEFAULT_SOCKET "../../groundStation/virtquad.socket"
-#define SOCKET_ENV "VIRT_QUAD_SOCKET"
-
-static int backendSocket;
-static int client;
+static char *fifo_full_name_rx;
+static char *fifo_full_name_tx;
+static int fifo_rx;
 
 int unix_uart_reset(struct UARTDriver *self) {
-	char * backend_socket_path = DEFAULT_SOCKET;
-	if (getenv(SOCKET_ENV)) {
-		backend_socket_path = getenv(SOCKET_ENV);
-	}
-
-	/* Unlink if it exists */
-	unlink(backend_socket_path);
-	printf("using socket '%s'\n", backend_socket_path);
-
-	/* Create socket */
-	backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+  fifo_full_name_rx = VIRT_QUAD_FIFOS_DIR "/uart-rx";
+  fifo_full_name_tx = VIRT_QUAD_FIFOS_DIR "/uart-tx";
 
+  char fifoname[64];
+  mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
 
-	/* Create sockaddr and bind */
-	struct sockaddr_un sa;
-	sa.sun_family = AF_UNIX;
-	strncpy(sa.sun_path, backend_socket_path, strlen(backend_socket_path));
-	sa.sun_path[strlen(backend_socket_path)+1] = '\0';
-	if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
-		err(-1, "bind");
-	}
+  unlink(fifo_full_name_rx);
+  mkfifo(fifo_full_name_rx, 0666);
+  fifo_rx = open(fifo_full_name_rx, O_RDONLY | O_NONBLOCK);
 
-	/* Listen */
-	if (listen(backendSocket, 1)) {
-		err(-1, "listen");
-	}
+  unlink(fifo_full_name_tx);
+  mkfifo(fifo_full_name_tx, 0666);
 
-	printf("Waiting for backend to connect\n");
-
-	while (1) {
-		client = accept(backendSocket, NULL, NULL);
-		if (client > 0)
-			break;
-	}
-	printf("backend connection found on socket %d.\n", client);
   return 0;
 }
 
 int unix_uart_write(struct UARTDriver *self, unsigned char c) {
-	send(client, &c, 1, MSG_DONTWAIT);
-	return 0;
+  int fifo = open(fifo_full_name_tx, O_WRONLY | O_NONBLOCK);
+  if (fifo >= 0) {
+    printf("%s: %x\n", "uart-tx", c);
+    write(fifo, &c, 1);
+  }
+  close(fifo);
+  return 0;
 }
 
 int unix_uart_read(struct UARTDriver *self, unsigned char *c) {
-	int bytes_available;
-	ioctl(client,FIONREAD,&bytes_available);
-	
-	if (bytes_available > 0) {
-		int bytes = recv(client, c, 1, 0);
-		return 0;
-	} else {
-		return 1;
-	}
-}
\ No newline at end of file
+  int err = read(fifo_rx, c, 1);
+  if (err > 0) {
+    printf("%s: %x\n", "uart-rx", *c);
+  }
+  return err <= 0;
+}

quad/xsdk_workspace/README.md

-# Branch for hardware/software designs for Quadcopter
+# XSDK Workspace
+
+This directory is reserved for XSDK projects.
+
+## Setup
+XSDK, being based on Ecplise, is rather fragile, so do yourself a favor
+and read this section so things get setup correctly.
+
+1. When you first open eclipse, select this directory, xsdk_workspace, as
+   your workspace (see what we did there?).
+
+2. When you get to your workbench, your project pane should be empty. To
+   add these projects, right-click on the project pane, and click on something
+   like "import projects".
+
+  1. Select "Import Existing Projects" (whereever that is)
+  2. And then select the xsd_workspace as the folder where you want to import
+     projects. Add them all.
+
+3. If things are going swimmingly, then you should be able to build everyhing
+   and be off on your merry embedded endeavors.
\ No newline at end of file