File content as of revision 12:11a62ab9bfed:

#include "UsaPack.hpp"
#include "mbed.h"

void UsaPack::Receive()
{
    while (serial.readable())
    {
        char c;
        c = serial.getc();
        receive_buffer[receive_index] = c;
        receive_index = (receive_index + 1) % receive_size;
        if (c == '\0')
        {
            Decode();
            receive_index = 0;
        }
    }
}

void UsaPack::SendByte()
{
    while (serial.writable())
    {
        if (send_index != send_end_index)
        {
            serial.putc(send_buffer[send_index]);
            send_index = (send_index + 1) % send_size;
        }
        else
        {
            serial.attach(NULL, Serial::TxIrq);
            break;
        }
    }
}

void UsaPack::Decode()
{
    char data[receive_size];
    CobsDecode(receive_buffer, receive_index-2, data);
    union c4
    {
        char c[4];
        int a;
    };
    c4 _address;
    c4 _checksum;
    for (int i = 0; i < 4; i++)
    {
        _address.c[i] = data[i];
    }
    for (int i = 0; i < 4; i++)
    {
        _checksum.c[i] = data[i+4];
    }
    for (int i = 0; i < package_index; i++)
    {
        if (package_address[i] == _address.a)
        {
            int checksum = 0;
            for (int j = 0; j < package_size[i]; j++)
            {
                checksum += data[j+8];
            }
            if (_checksum.a != checksum)
            {
                return;
            }
            for (int j = 0; j < package_size[i]; j++)
            {
                *((char*)(package_object[i])+j) = data[j+8];
            }
            break;
        }
    }
}

UsaPack::UsaPack(PinName tx, PinName rx, int baud)
    :serial(tx, rx, baud), receive_index(0), package_index(0), send_index(0), send_end_index(0)
{
    serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
    for (int i = 0; i < receive_size; i++)
    {
        receive_buffer[i] = '\0';
    }
}

void UsaPack::CobsEncode(char data[], int length, char send_data[])
{
    int last_zero = 0;
    for (int i = 0; i < length; i++)
    {
        if (data[i] == 0)
        {
            send_data[last_zero] = i - last_zero + 1;
            last_zero = i + 1;
        }
        else
        {
            send_data[i + 1] = data[i];
        }
    }
    send_data[last_zero] = length - last_zero + 1;
    send_data[length + 1] = 0;
}

void UsaPack::CobsDecode(char receive_data[], int length, char data[])
{
    int next_zero;
    next_zero = receive_data[0] - 1;
    for (int i = 0; i < length; i++)
    {
        data[i] = receive_data[i + 1];
    }
    while (next_zero < length)
    {
        data[next_zero] = 0;
        next_zero += receive_data[next_zero + 1];
    }
}