Colony

#include "comm_server.h"

#include <avr/pgmspace.h>

#include "global.h"

#include "tests.h"

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

// ## Settings ##

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

#define comm_server_debug

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

// ## Constants ##

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

#define buffer_size 120

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

// ## Initialization ##

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

void comm_server_init ()

{

        usb_init ();

}

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

// ## Message processing ##

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

// This function is copied from XNS

//static void serial_send_string_P (const char *s)/*{{{*/

//{

//    char buf;

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

//    {

//        serial_transmit (buf);

//        s++;

//    }

//}

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

// This function is copied from XNS

static bool serial_match (PGM_P text, uint8_t *buffer, uint8_t size)

{

        uint8_t text_len=strlen_P (text);

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

        if (size<text_len) return false;

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

        if (size>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 (uint8_t *buffer, uint8_t size)

{

        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 (uint8_t *buffer, uint8_t size)

{

        // FIXME parameter handling

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

        test_all ();

}

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

// ## Messages (receiving) ##

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

// 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").

const char command_ping          [] PROGMEM = "ping"      ;

const char command_start_test    [] PROGMEM = "start_test";

static void handle_message (uint8_t *message, uint8_t size)

{

#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, size)) { handle_message_ping       (message, size); handled=true; }

        if (serial_match (command_start_test, message, size)) { handle_message_start_test (message, size); 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)

{

        uint8_t 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, buffer_fill);

                                // 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;

                        }

                }

        }

}