File content as of revision 3:0c4aa3cec685:

#include "CommandPacket.h"
#include "mbed.h"

#define CHECKSUM_DEBUG

uint8_t generate_checksum(uint8_t *data, uint8_t length, uint8_t offset)
{
    uint8_t s = 0;
    
    if (data)
    {
        // take the sum of all data bytes preceding checksum field
        for(uint8_t i = 0; i < length; i++)
        {
            s += data[i + offset];
        }
        
        // calculate two's complement of the remainder
        s = 0xff - s + 1;
    }
    
    return s;
}

uint8_t hexchar_to_uint8(uint8_t ch)
{
    uint8_t val = 0;

    if (ch >= '0' && ch <= '9')
    {
        val = ch - '0';
    }
    else if (ch >= 'A' && ch <= 'F')
    {
        val  = ch - 'A';
        val += 10;
    }
    else if (ch >= 'a' && ch <= 'f')
    {
        val = ch - 'a';
        val += 10;
    }

    return val;
}
    

bool CommandPacket::verify()
{
    return checksum == generate_checksum();
}

CommandPacket::CommandPacket()
{
    // reset internal error number
    errno = NO_ERROR;
    
    // set public variables
    sflag = '<';
    command = 0x0;
    length = 0x0;
    memset(payload, 0x0, sizeof(payload));
    checksum = 0x0;
    eflag = '>';
}

const char *CommandPacket::getErrorString() const
{
    return errorString[errno];
}

int CommandPacket::serialize(uint8_t *output)
{    
    // create buffer for payload
    uint8_t buffer[length * 2];
    memset(buffer, 0x0, sizeof(buffer));
    
    for (int i = 0; i < length; i++)
    {
        sprintf((char *) (buffer + i * 2), "%02X", payload[i]);
    }
    
    // assume the user provide output buffer large enough
    sprintf((char *) output, "%c%02X%02X%s%02X%c",
        sflag,
        command,
        length,
        (char *) buffer,
        generate_checksum(),
        eflag
    );
    
    return length;
}

uint8_t CommandPacket::generate_checksum()
{
    // checksum is defined by the sum of all characters between sflag and checksum field
    uint8_t s = 0;
    
    // include command
    s += command;
    
    // include length
    s += length;
    
    // include payload
    for (int i = 0; i < length; i++)
    {
        s += payload[i];
    }
    
    // calculate two's complement of the remainder
    s = 0xff - s + 1;
    
    return s;
}