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.
108 lines
1.9 KiB
C++
108 lines
1.9 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
|
|
{
|
|
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
|
|
bool ps_packet::decode()
|
|
{
|
|
bool ret = true;
|
|
do
|
|
{
|
|
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
|
|
bool ps_packet::validate()
|
|
{
|
|
bool ret = true;
|
|
do
|
|
{
|
|
|
|
} while (0);
|
|
return ret;
|
|
}
|
|
void ps_packet::calc_checksum()
|
|
{}
|
|
|
|
bool ps_packet::stuff_bytes()
|
|
{
|
|
int old_ind = 0;
|
|
int new_ind = 0;
|
|
uint8_t temp_buffer[MAX_FRAME_DATA_LEN];
|
|
memcpy(frame_data, temp_buffer, 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];
|
|
}
|
|
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));
|
|
}
|