Command reading code

emb/command.c

+#include <stdio.h>
+#include <stdlib.h>
+#include "command.h"
+
+#define NUM_COMMANDS 20
+
+/* Cyclic buffer; when comm_start == comm_end, length = 0 */
+struct command command_buf[NUM_COMMANDS];
+uint16_t comm_start = 0, comm_end = 0;
+
+/* Add y to x, wrapping around limit */
+static inline void add_wrap(uint16_t *x, uint16_t y, uint16_t limit)
+{
+     *x += y;
+     if (*x >= limit) *x %= limit;
+     while (*x < 0) *x += limit;
+}
+
+enum state {
+     STATE_COMM, STATE_COMM_SPACE, STATE_ARG
+};
+
+/* Return 1 if new command ready */
+uint8_t uart_add_ch(char c)
+{
+     static enum state state = STATE_COMM;
+     static char argbuf[20]; /* Buffer for storing argument */
+     static int argbuf_ind = 0; /* Index into argbuf of next character */
+     static int argnum = 0;
+     struct command *curr = &command_buf[comm_end];
+     
+     switch (state) {
+     case STATE_COMM:
+	  curr->comm_ch = c;
+	  state = STATE_COMM_SPACE;
+	  break;
+     case STATE_COMM_SPACE:
+	  if (c == ' ') {
+	       state = STATE_ARG;
+	  } else {
+	       state = STATE_COMM;
+	       add_wrap(&comm_end, 1, NUM_COMMANDS);
+	  }
+	  break;
+     case STATE_ARG:
+	  if (c == ' ' || c == '\n') {
+	       argbuf[argbuf_ind] = '\0'; /* Terminate string */
+	       curr->arg[argnum] = atoi(argbuf);
+	       argbuf_ind = 0;
+	       if (c == ' ') { /* More arguments follow */
+		    argnum++;
+	       } else { /* End of line */
+		    argnum = 0;
+		    state = STATE_COMM;
+		    add_wrap(&comm_end, 1, NUM_COMMANDS);
+	       }
+	  } else {
+	       if (argbuf_ind < 19) argbuf[argbuf_ind++] = c;
+	  }
+	  break;
+     }
+     return state == STATE_COMM;
+}
+
+struct command *get_command(void)
+{
+     if (comm_start != comm_end) {
+	  struct command *ret = &command_buf[comm_start];
+	  add_wrap(&comm_start, 1, NUM_COMMANDS);
+	  return ret;
+     } else {
+	  return NULL;
+     }
+}

emb/command.h

+#ifndef _COMMAND_H
+#define _COMMAND_H
+
+#include <stdint.h>
+
+struct command {
+     char comm_ch; /* Character of command (ie 's', 'v', 'd') */
+     int16_t arg[2];
+};
+
+uint8_t uart_add_ch(char c);
+struct command *get_command(void);
+
+#endif /* _COMMAND_H */