Revision 1198
Added server communication parameter parsing
Misc. fixes
| comm_server.c | ||
|---|---|---|
| 1 | 1 |
#include "comm_server.h" |
| 2 | 2 |
|
| 3 | 3 |
#include <avr/pgmspace.h> |
| 4 |
#include <string.h> |
|
| 4 | 5 |
|
| 5 | 6 |
#include "global.h" |
| 6 | 7 |
#include "tests.h" |
| ... | ... | |
| 9 | 10 |
// ## Settings ## |
| 10 | 11 |
// ############## |
| 11 | 12 |
|
| 12 |
#define comm_server_debug |
|
| 13 |
//#define comm_server_debug
|
|
| 13 | 14 |
|
| 14 | 15 |
// ############### |
| 15 | 16 |
// ## Constants ## |
| ... | ... | |
| 17 | 18 |
|
| 18 | 19 |
#define buffer_size 120 |
| 19 | 20 |
|
| 21 |
// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see |
|
| 22 |
// the avr-libc documentation, chapter "Data in Program Space"). |
|
| 20 | 23 |
|
| 24 |
// Commands |
|
| 25 |
const char command_ping [] PROGMEM = "ping" ; |
|
| 26 |
const char command_start_test [] PROGMEM = "start_test" ; |
|
| 27 |
const char command_all [] PROGMEM = "all" ; |
|
| 28 |
const char command_comm [] PROGMEM = "comm" ; |
|
| 29 |
const char command_bom [] PROGMEM = "bom" ; |
|
| 30 |
const char command_rangefinder [] PROGMEM = "rangefinder"; |
|
| 31 |
const char command_motor [] PROGMEM = "motor" ; |
|
| 32 |
const char command_encoder [] PROGMEM = "encoder" ; |
|
| 33 |
const char command_help [] PROGMEM = "help" ; |
|
| 34 |
|
|
| 35 |
// Help |
|
| 36 |
const char help_text [] PROGMEM = |
|
| 37 |
"# Available commands:" NL |
|
| 38 |
"# - help" NL |
|
| 39 |
"# - ping" NL |
|
| 40 |
"# - start_test all" NL |
|
| 41 |
"# - start_test comm" NL |
|
| 42 |
"# - start_test bom all|<num> (0..15)" NL |
|
| 43 |
"# - start_test rangefinder all|<num> (?..?)" NL |
|
| 44 |
"# - start_test motor all|<num> (1..2)" NL |
|
| 45 |
"# - start_test encoder all|<num> (1..2)" NL |
|
| 46 |
; |
|
| 47 |
|
|
| 48 |
|
|
| 21 | 49 |
// #################### |
| 22 | 50 |
// ## Initialization ## |
| 23 | 51 |
// #################### |
| ... | ... | |
| 32 | 60 |
// ## Message processing ## |
| 33 | 61 |
// ######################## |
| 34 | 62 |
|
| 35 |
// This function is copied from XNS
|
|
| 36 |
//static void serial_send_string_P (const char *s)/*{{{*/
|
|
| 37 |
//{
|
|
| 38 |
// char buf;
|
|
| 39 |
// while (memcpy_P (&buf, s, sizeof (char)), buf!=0)
|
|
| 40 |
// {
|
|
| 41 |
// serial_transmit (buf);
|
|
| 42 |
// s++;
|
|
| 43 |
// }
|
|
| 44 |
//}
|
|
| 63 |
// TODO move this to the library
|
|
| 64 |
static void serial_send_string_P (PGM_P s)
|
|
| 65 |
{
|
|
| 66 |
char buf; |
|
| 67 |
while (memcpy_P (&buf, s, sizeof (char)), buf!=0) |
|
| 68 |
{
|
|
| 69 |
usb_putc (buf);
|
|
| 70 |
s++; |
|
| 71 |
} |
|
| 72 |
} |
|
| 45 | 73 |
|
| 74 |
static char *find_next_parameter (char *p) |
|
| 75 |
{
|
|
| 76 |
// Advance p until a blank or the terminating zero is found. |
|
| 77 |
while ((*p)!=' ' && (*p)!=0) p++; |
|
| 78 |
|
|
| 79 |
// Advance p until a non-blank (including the terminating zero) is found. |
|
| 80 |
while (*p==' ') p++; |
|
| 81 |
|
|
| 82 |
// We have found the next parameter unless p points to the terminating zero. |
|
| 83 |
if (*p==0) |
|
| 84 |
return NULL; |
|
| 85 |
else |
|
| 86 |
return p; |
|
| 87 |
} |
|
| 88 |
|
|
| 46 | 89 |
/** Check if the buffer starts with the given text, followed by a space */ |
| 47 |
// This function is copied from XNS |
|
| 48 |
static bool serial_match (PGM_P text, uint8_t *buffer, uint8_t size) |
|
| 90 |
static bool serial_match (PGM_P text, const char *buffer) |
|
| 49 | 91 |
{
|
| 50 | 92 |
uint8_t text_len=strlen_P (text); |
| 93 |
uint8_t buffer_len=strlen (buffer); |
|
| 51 | 94 |
|
| 52 | 95 |
// If the buffer is shorter than the text, there is no match |
| 53 |
if (size<text_len) return false;
|
|
| 96 |
if (buffer_len<text_len) return false;
|
|
| 54 | 97 |
|
| 55 | 98 |
// If the buffer is longer than the text, it must have a space at position after the text |
| 56 |
if (size>text_len && buffer[text_len]!=' ') return false;
|
|
| 99 |
if (buffer_len>text_len && buffer[text_len]!=' ') return false;
|
|
| 57 | 100 |
|
| 58 | 101 |
// Test for match |
| 59 | 102 |
if (strncmp_P ((char *)buffer, text, text_len)==0) return true; |
| ... | ... | |
| 80 | 123 |
// ## Message handlers ## |
| 81 | 124 |
// ###################### |
| 82 | 125 |
|
| 83 |
static void handle_message_ping (uint8_t *buffer, uint8_t size)
|
|
| 126 |
static void handle_message_ping (char *buffer)
|
|
| 84 | 127 |
{
|
| 85 | 128 |
usb_puts ("pong" NL);
|
| 86 | 129 |
orbs_set (255, 127, 0, 0, 255, 0); |
| ... | ... | |
| 88 | 131 |
orbs_set (0, 255, 0, 255, 127, 0); |
| 89 | 132 |
} |
| 90 | 133 |
|
| 91 |
static void handle_message_start_test (uint8_t *buffer, uint8_t size)
|
|
| 134 |
static void handle_message_start_test (char *buffer)
|
|
| 92 | 135 |
{
|
| 93 | 136 |
// FIXME parameter handling |
| 94 | 137 |
// Parameters: all/rangefinder/motor/encoder all/<num> |
| 95 |
test_all (); |
|
| 138 |
|
|
| 139 |
buffer=find_next_parameter (buffer); |
|
| 140 |
|
|
| 141 |
if (!buffer) |
|
| 142 |
usb_puts ("# Parameter missing" NL);
|
|
| 143 |
else if (serial_match (command_all, buffer)) |
|
| 144 |
{
|
|
| 145 |
usb_puts ("# Doing all tests" NL);
|
|
| 146 |
test_all (); |
|
| 147 |
} |
|
| 148 |
else |
|
| 149 |
{
|
|
| 150 |
usb_puts ("# Warning: unhandled parameters: [");
|
|
| 151 |
usb_puts (buffer); |
|
| 152 |
usb_puts ("]" NL);
|
|
| 153 |
} |
|
| 96 | 154 |
} |
| 97 | 155 |
|
| 156 |
static void handle_message_help (char *buffer) |
|
| 157 |
{
|
|
| 158 |
serial_send_string_P (help_text); |
|
| 159 |
} |
|
| 98 | 160 |
|
| 161 |
|
|
| 99 | 162 |
// ########################## |
| 100 | 163 |
// ## Messages (receiving) ## |
| 101 | 164 |
// ########################## |
| 102 | 165 |
|
| 103 |
|
|
| 104 |
// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see |
|
| 105 |
// the avr-libc documentation, chapter "Data in Program Space"). |
|
| 106 |
const char command_ping [] PROGMEM = "ping" ; |
|
| 107 |
const char command_start_test [] PROGMEM = "start_test"; |
|
| 108 |
|
|
| 109 |
static void handle_message (uint8_t *message, uint8_t size) |
|
| 166 |
/** message is 0 terminated */ |
|
| 167 |
static void handle_message (char *message) |
|
| 110 | 168 |
{
|
| 111 | 169 |
#ifdef comm_server_debug |
| 112 | 170 |
// Output: "# Received a message, size <size>: [<message>]" |
| ... | ... | |
| 118 | 176 |
#endif |
| 119 | 177 |
|
| 120 | 178 |
bool handled=false; |
| 121 |
if (serial_match (command_ping , message, size)) { handle_message_ping (message, size); handled=true; }
|
|
| 122 |
if (serial_match (command_start_test, message, size)) { handle_message_start_test (message, size); handled=true; }
|
|
| 179 |
if (serial_match (command_ping , message)) { handle_message_ping (message); handled=true; }
|
|
| 180 |
if (serial_match (command_start_test, message)) { handle_message_start_test (message); handled=true; }
|
|
| 181 |
if (serial_match (command_help , message)) { handle_message_help (message); handled=true; }
|
|
| 123 | 182 |
// More messages go here |
| 124 | 183 |
|
| 125 | 184 |
if (!handled) |
| ... | ... | |
| 155 | 214 |
|
| 156 | 215 |
void server_main (void) |
| 157 | 216 |
{
|
| 158 |
uint8_t buffer[buffer_size];
|
|
| 217 |
char buffer[buffer_size];
|
|
| 159 | 218 |
uint8_t c; |
| 160 | 219 |
uint8_t buffer_fill=0; |
| 161 | 220 |
|
| ... | ... | |
| 181 | 240 |
buffer[buffer_fill]=0; |
| 182 | 241 |
|
| 183 | 242 |
// Handle the message. |
| 184 |
handle_message (buffer, buffer_fill);
|
|
| 243 |
handle_message (buffer); |
|
| 185 | 244 |
|
| 186 | 245 |
// Reset the buffer |
| 187 | 246 |
buffer_fill=0; |
Also available in: Unified diff