CmdBuffer.h

00001 #ifndef CMDBUFFER_H
00002 #define CMDBUFFER_H
00003 
00004 #define MAX_CMD_BUF 16
00005 #define MAX_CMD_NUM 0xb
00006 #define MAX_INTERRUPTS 3
00007 
00008 // Command Struct
00009 typedef struct {
00010     char cmd;
00011     union {
00012         float f[6];
00013         long l[6];
00014     };       
00015 } Command;
00016 
00017 class CmdBuffer {
00018 public:
00019     CmdBuffer() {
00020         write = 0;
00021         read = 0;
00022         size = MAX_CMD_BUF + 1;
00023         temp_ptr = 0;
00024         temp_size = 2 + 6*9;
00025         cmd_sizes[0x0] = 3;
00026         cmd_sizes[0x1] = 5;
00027         cmd_sizes[0x2] = 6;
00028         cmd_sizes[0x3] = 6;
00029         cmd_sizes[0x4] = 1;
00030         cmd_sizes[0x5] = 1;
00031         cmd_sizes[0x6] = 4;
00032         cmd_sizes[0x7] = 1;
00033         cmd_sizes[0x8] = 0;
00034         cmd_sizes[0x9] = 0;
00035         cmd_sizes[0xa] = 0;
00036         interrupt_cmds[0] = 0xff;
00037         interrupt_cmds[1] = 0xff;
00038         interrupt_cmds[2] = 0xff;
00039     };
00040 
00041     bool isFull() {
00042         return ((write + 1) % size == read);
00043     };
00044 
00045     bool isEmpty() {
00046         return (read == write);
00047     };
00048 
00049     // Return true if we've queued a command
00050     bool queue(char k) {
00051         tempbuf[temp_ptr++] = k;
00052         if (temp_ptr == 2) {
00053             int cmd_num = (0x0f&char_to_num(tempbuf[1]))|(0xf0&char_to_num(tempbuf[0])<<4);
00054             if (cmd_num > MAX_CMD_NUM) { //Invalid
00055                 temp_ptr = 0;
00056                 temp_size = 4 + 6*9;
00057                 return true;
00058             } else {
00059                 temp_size = 2 + 9 * cmd_sizes[cmd_num];
00060             }
00061         }
00062         if (temp_ptr >= temp_size) {
00063             Command c;
00064             c.cmd = (0x0f&char_to_num(tempbuf[1]))|(0xf0&char_to_num(tempbuf[0])<<4);
00065             for (int i = 0; i < cmd_sizes[c.cmd]; i++) {
00066                 c.l[i] = str_to_long(tempbuf + 3 + i*9);
00067 #ifdef DEBUG
00068                 printf("arg%d: %x\n\r",i,c.l[i]);
00069 #endif
00070             }
00071             for (int i = 0; i < MAX_INTERRUPTS; i++) {
00072                 if (c.cmd == interrupt_cmds[i]){
00073                     interrupt_funcs[i](&c);
00074                 }
00075             }
00076             queue_cmd(c);
00077             temp_ptr = 0;
00078             temp_size = 4 + 6*9;
00079             return true;
00080         }
00081         return false;
00082     }
00083 
00084     uint16_t available() {
00085         return (write >= read) ? write - read : size - read + write;
00086     };
00087 
00088     Command * dequeue() {
00089         Command * c = NULL;
00090         if (!isEmpty()) {
00091             c = &buf[read++];
00092             read %= size;
00093         }
00094         return c;
00095     };
00096     
00097     bool attach_interrupt(char cmd, void (*func)(Command *), int num) {
00098         if (num < MAX_INTERRUPTS) {
00099             interrupt_funcs[num] = func;
00100             interrupt_cmds[num] = cmd;
00101             return true;
00102         }
00103         return false;
00104     }
00105 
00106 private:
00107     void queue_cmd(Command k) {
00108         if (isFull()) {
00109             read++;
00110             read %= size;
00111         }
00112         buf[write++] = k;
00113         write %= size;
00114     }
00115     
00116     // Return 0xf if char is invalid
00117     char char_to_num(char c) {
00118         if(c >= '0' && c <= '9') {
00119             return c - '0';
00120         } else if(c >= 'a' && c <= 'f') {
00121             return c - 'a' + 0xa;
00122         } else if(c >= 'A' && c <= 'F') {
00123             return c - 'A' + 0xa;
00124         } else {
00125 #ifdef DEBUG
00126             printf("CmdBuffer::char_to_num: invalid number\r\n");
00127 #endif
00128            return 0xf;
00129         }      
00130     }
00131 
00132     long str_to_long(char * str) {
00133         long ans = 0;
00134         for (int i = 7; (i > 0 && i < 8); i -= 2) {
00135             ans = ans << 8;
00136             ans |= (0xf0 & char_to_num(str[i - 1])<<4);
00137             ans |= (0x0f & char_to_num(str[i]));
00138         }
00139         return ans;
00140     }
00141 
00142     volatile uint16_t write;
00143     volatile uint16_t read;
00144     uint16_t size;
00145     void (*interrupt_funcs[MAX_INTERRUPTS])(Command *);
00146     char interrupt_cmds[MAX_INTERRUPTS];
00147     Command buf[MAX_CMD_BUF + 1];
00148     volatile uint16_t temp_ptr;
00149     volatile uint16_t temp_size;
00150     char tempbuf[2 + 6*9 + 1];
00151     uint16_t cmd_sizes[MAX_CMD_NUM];
00152 };
00153 
00154 #endif