File content as of revision 0:61c402886fbb:

#include "YDwifi_uartmsg.h"

C_YD_UartMsg::C_YD_UartMsg()
{
}

unsigned short C_YD_UartMsg::makePayload( unsigned int cmd_len, unsigned char *cmd_p, unsigned char *payload_p )
{
    unsigned short payload_len = 0;
    int i;
    
    for( i = 0; i < cmd_len; i++, payload_p++, payload_len++ )
    {
        /* check Escape code */
        if( ( cmd_p[i] == UART_CMD_SOM ) || ( cmd_p[i] == UART_CMD_EOM ) || ( cmd_p[i] == UART_CMD_ESC ) )
        {
            /* Add ESC */
            *payload_p = UART_CMD_ESC;
            payload_len++;
            
            payload_p++;
            *payload_p = (0x80 | cmd_p[i]);
        }
        else
        {
            *payload_p = cmd_p[i];
        }
    }
    
    return payload_len;
}

unsigned int C_YD_UartMsg::makeRequest( unsigned char cmd_id,unsigned char *payload_p
                                , unsigned short payload_len, unsigned char *uart_command_p )
{
    unsigned char check_sum = 0;    // Check Sum
    unsigned int  uart_cmd_len = 0;
    int i;
    
    // set SOM
    *uart_command_p = UART_CMD_SOM;
    uart_command_p++;
    uart_cmd_len++;
    
    // set payload length L0
    *uart_command_p = (0x80 | (payload_len & 0x007f));
    check_sum += *uart_command_p;
    uart_command_p++;
    uart_cmd_len++;

    // set payload length L1
    *uart_command_p = (0x80 | ( (payload_len >> 7) & 0x003f));
    check_sum += *uart_command_p;
    uart_command_p++;
    uart_cmd_len++;
    
    // set Command ID
    *uart_command_p = (0x80 | cmd_id);
    check_sum += *uart_command_p;
    uart_command_p++;
    uart_cmd_len++;

    // set Payload
    for( i = 0; i < payload_len; i++, uart_command_p++, uart_cmd_len++ )
    {
        *uart_command_p = payload_p[i];
        check_sum += *uart_command_p;
    }

    // set Check sum
    *uart_command_p = (0x80 | check_sum);
    uart_command_p++;
    uart_cmd_len++;
    
    // set EOM
    *uart_command_p = UART_CMD_EOM;
    uart_cmd_len++;
    
    return uart_cmd_len;
}