Colony

#include "comm_server.h"

#include <avr/pgmspace.h>

#include <string.h>

#include "global.h"

#include "tests.h"

// ##############

// ## Settings ##

// ##############

//#define comm_server_debug

// ###############

// ## Constants ##

// ###############

#define buffer_size 120

// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see

// the avr-libc documentation, chapter "Data in Program Space").

// Commands 

const char command_ping          [] PROGMEM = "ping"       ;

const char command_start_test    [] PROGMEM = "start_test" ;

const char command_all           [] PROGMEM = "all"        ;

const char command_comm          [] PROGMEM = "comm"       ;

const char command_bom           [] PROGMEM = "bom"        ;

const char command_rangefinder   [] PROGMEM = "rangefinder";

const char command_motor         [] PROGMEM = "motor"      ;

const char command_encoder       [] PROGMEM = "encoder"    ;

const char command_help          [] PROGMEM = "help"       ;

// Help

const char help_text          [] PROGMEM =

        "# Available commands:" NL

        "#   - help" NL

        "#   - ping" NL

        "#   - start_test all" NL

        "#   - start_test comm" NL

        "#   - start_test bom all|<num> (0..15)" NL

        "#   - start_test rangefinder all|<num> (?..?)" NL

        "#   - start_test motor all|<num> (1..2)" NL

        "#   - start_test encoder all|<num> (1..2)" NL

        ;

// ####################

// ## Initialization ##

// ####################

void comm_server_init ()

{

        usb_init ();

}

// ########################

// ## Message processing ##

// ########################

// TODO move this to the library

static void serial_send_string_P (PGM_P s)

{

    char buf;

    while (memcpy_P (&buf, s, sizeof (char)), buf!=0)

    {

        usb_putc (buf);

        s++;

    }

}

static char *find_next_parameter (char *p)

{

        // Advance p until a blank or the terminating zero is found.

        while ((*p)!=' ' && (*p)!=0) p++;

        // Advance p until a non-blank (including the terminating zero) is found.

        while (*p==' ') p++;

        // We have found the next parameter unless p points to the terminating zero.

        if (*p==0)

                return NULL;

        else

                return p;

}

/** Check if the buffer starts with the given text, followed by a space */

static bool serial_match (PGM_P text, const char *buffer)

{

        uint8_t text_len=strlen_P (text);

        uint8_t buffer_len=strlen (buffer);

        // If the buffer is shorter than the text, there is no match

        if (buffer_len<text_len) return false;

        // If the buffer is longer than the text, it must have a space at position after the text

        if (buffer_len>text_len && buffer[text_len]!=' ') return false;

        // Test for match

        if (strncmp_P ((char *)buffer, text, text_len)==0) return true;

        return false;

}

// ########################

// ## Messages (sending) ##

// ########################

void server_send_finished ()

{

        usb_puts ("finished");

}

// ######################

// ## Message handlers ##

// ######################

static void handle_message_ping (char *buffer)

{

        usb_puts ("pong" NL);

        orbs_set (255, 127, 0, 0, 255, 0);

        delay_ms (400);

        orbs_set (0, 255, 0, 255, 127, 0);

}

static void handle_message_start_test (char *buffer)

{

        // FIXME parameter handling

        // Parameters: all/rangefinder/motor/encoder all/<num>

        buffer=find_next_parameter (buffer);

        if (!buffer)

                usb_puts ("# Parameter missing" NL);

        else if (serial_match (command_all, buffer))

        {

                usb_puts ("# Doing all tests" NL);

                test_all ();

        }

        else

        {

                usb_puts ("# Warning: unhandled parameters: [");

                usb_puts (buffer);

                usb_puts ("]" NL);

        }

}

static void handle_message_help (char *buffer)

{

        serial_send_string_P (help_text);

}

// ##########################

// ## Messages (receiving) ##

// ##########################

/** message is 0 terminated */

static void handle_message (char *message)

{

#ifdef comm_server_debug

        // Output: "# Received a message, size <size>: [<message>]"

        usb_puts ("# Received a message, size ");

        usb_puti(size);

        usb_puts (": [");

        usb_puts ((char *)message);

        usb_puts ("]" NL);

#endif

        bool handled=false;

        if (serial_match (command_ping      , message)) { handle_message_ping       (message); handled=true; }

        if (serial_match (command_start_test, message)) { handle_message_start_test (message); handled=true; }

        if (serial_match (command_help      , message)) { handle_message_help       (message); handled=true; }

        // More messages go here

        if (!handled)

        {

                usb_puts ("# Warning: unhandled message: [");

                usb_puts ((char *)message);

                usb_puts ("]" NL);

        }

}

// ###############

// ## Debugging ##

// ###############

void byte_transmission_test (void)

{

        uint8_t c;

        while (1)

        {

                c=usb_getc ();

                usb_puts ("[");

                usb_puti (c);

                usb_puts ("] ");

        }

}

// ###############

// ## Main loop ##

// ###############

void server_main (void)

{

        char buffer[buffer_size];

        uint8_t c;

        uint8_t buffer_fill=0;

        //byte_transmission_test (); // Does not return

        usb_puts (NL);

        usb_puts ("# Diagnostic station server mode" NL);

        while (1)

        {

                // Do nothing until we receive a byte (this function will return 0 when a character has been read)

                // (Yes, we could use the blocking function for now, but we may need the non-blocking later)

                while (usb_getc_nb ((char *)&c));

                if (c=='\r' || c=='\n')

                {

                        // A newline character was received. This terminates the message.

                        // Empty lines are ignored.

                        if (buffer_fill>0)

                        {

                                // Add a terminating 0 to the buffer so the string functions won't try to read outside the buffer.

                                buffer[buffer_fill]=0;

                                // Handle the message.

                                handle_message (buffer);

                                // Reset the buffer

                                buffer_fill=0;

                        }

                }

                else

                {

                        // If there is enough space left in the buffer, add the character we just received. Leave one byte for a

                        // terminating 0. If there is not enough space in the buffer, the rest of the message is ignored.

                        if (buffer_fill<buffer_size-1)

                        {

                                buffer[buffer_fill]=c;

                                ++buffer_fill;

                        }

                }

        }

}