File content as of revision 0:8adbff0ff843:

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

void UsaPack::Receive()
{
    
}

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

void UsaPack::CobsEncode(uint8_t data[], int length, uint8_t 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(uint8_t receive_data[], int length, uint8_t 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];
    }
}