You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
163 lines
3.1 KiB
C++
163 lines
3.1 KiB
C++
#include "p_serial_packet.h"
|
|
#include <iostream>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
ps_packet::ps_packet(uint8_t* data)
|
|
{}
|
|
|
|
ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
|
|
uint8_t* _frame_data, uint8_t _frame_data_len)
|
|
{
|
|
data[0] = 0x7E;
|
|
src_addr = &data[1];
|
|
dest_addr = &data[2];
|
|
frame_data_len = &data[3];
|
|
frame_data = &data[4];
|
|
msg_len = 5 + _frame_data_len;
|
|
checksum = nullptr;
|
|
|
|
*src_addr = _src_addr;
|
|
*dest_addr = _dest_addr;
|
|
*frame_data_len = _frame_data_len;
|
|
|
|
memset(frame_data, 0, MAX_FRAME_DATA_LEN);
|
|
memcpy(frame_data, _frame_data, _frame_data_len);
|
|
}
|
|
ps_packet::~ps_packet()
|
|
{}
|
|
|
|
// takes members and prepares the buffer for transmission
|
|
bool ps_packet::encode()
|
|
{
|
|
bool ret = true;
|
|
do
|
|
{
|
|
uint8_t csum = calc_checksum();
|
|
if (!stuff_bytes())
|
|
{
|
|
ret = false;
|
|
break;
|
|
}
|
|
checksum = &data[msg_len - 1];
|
|
*checksum = csum;
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
|
|
bool ps_packet::decode()
|
|
{
|
|
bool ret = true;
|
|
do
|
|
{
|
|
if (!destuff_bytes())
|
|
{
|
|
ret = false;
|
|
break;
|
|
}
|
|
|
|
if (!validate())
|
|
{
|
|
ret = false;
|
|
break;
|
|
}
|
|
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
|
|
// done after receiving a packet and destuffing it
|
|
bool ps_packet::validate()
|
|
{
|
|
bool ret = true;
|
|
do
|
|
{
|
|
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
|
|
uint8_t ps_packet::calc_checksum()
|
|
{
|
|
uint32_t ret = 0;
|
|
for (size_t ind = 0; ind < *frame_data_len; ind++)
|
|
{
|
|
ret += frame_data[ind];
|
|
}
|
|
ret &= 0xFF;
|
|
ret = 0xFF - ret;
|
|
return (uint8_t)ret;
|
|
}
|
|
|
|
bool ps_packet::stuff_bytes()
|
|
{
|
|
int old_ind = 0;
|
|
int new_ind = 0;
|
|
uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0};
|
|
memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN);
|
|
msg_len = *frame_data_len + 5;
|
|
for (; old_ind < *frame_data_len; old_ind++)
|
|
{
|
|
if (temp_buffer[old_ind] == 0x7E || temp_buffer[old_ind] == 0x7D)
|
|
{
|
|
msg_len++;
|
|
frame_data[new_ind++] = 0x7D;
|
|
frame_data[new_ind++] = temp_buffer[old_ind] ^ 0x20;
|
|
}
|
|
else
|
|
{
|
|
frame_data[new_ind++] = temp_buffer[old_ind];
|
|
}
|
|
}
|
|
if (msg_len > MAX_MESSAGE_LEN)
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ps_packet::destuff_bytes()
|
|
{
|
|
return false;
|
|
}
|
|
|
|
ps_packet ps_packet::from_xy(int32_t x, int32_t y)
|
|
{
|
|
uint8_t frame_data[MAX_FRAME_DATA_LEN];
|
|
memset(frame_data, 0, MAX_FRAME_DATA_LEN);
|
|
// op command
|
|
// 0x01 = set xy
|
|
frame_data[0] = 0x01; // + 1
|
|
memcpy(&frame_data[1], &x, 4); // + sizeof(x)
|
|
memcpy(&frame_data[5], &y, 4); // + sizeof(y)
|
|
|
|
return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
|
|
}
|
|
|
|
ps_packet ps_packet::test_packet()
|
|
{
|
|
uint8_t frame_data[16] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x7D, 0x7D, 0x07,
|
|
0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0F, 0x7E};
|
|
|
|
return ps_packet(MASTER_ADDR, 0xFF, frame_data, 16);
|
|
}
|
|
|
|
#ifdef DEBUG
|
|
void ps_packet::show_packet() const
|
|
{
|
|
printf("Start Byte:\t\t0x%02x\n", data[0]);
|
|
printf("Source Address:\t\t0x%02x\n", *src_addr);
|
|
printf("Dest Address:\t\t0x%02x\n", *dest_addr);
|
|
printf("Frame Data Len:\t\t0x%02x\n", *frame_data_len);
|
|
printf("Frame Data:\n");
|
|
for (size_t ind = 0; ind < msg_len - 4; ind++)
|
|
{
|
|
printf("\t\t[%2lu]: 0x%02x\n", ind, frame_data[(int)ind]);
|
|
}
|
|
printf("\n");
|
|
printf("Message Length: %lu\n", msg_len);
|
|
printf("Checksum: %0x02x\n", *checksum);
|
|
}
|
|
#endif
|