void osuar_comm_handle_receive(uint8_t *rxbuf, uint8_t num, osuar_rb_t *recv_rb)
{
static uint8_t msg_type; /* Received uplink message type */
static uint8_t msg[MSG_SIZE_MAX]; /* Received message */
static uint8_t r;
/* Add received bytes to ring buffer. */
osuar_rb_add(recv_rb, num, rxbuf); /* TODO(yoos): handle failure */
/* Try to parse message */
r = protocol_unpack(recv_rb, &msg_type, msg);
if (r) {
switch(msg_type) {
case(UP_COMMAND_TYPE):
memcpy(&g_cmd, msg, sizeoftype(msg_type));
break;
case(UP_CONFIG_TYPE):
/* TODO(yoos) */
break;
default:
break;
}
g_cmd.mode = r;
}
}
static int CreatePack(lua_State *L, int arg, size_t alignment)
{
int err;
char *ptr;
int type = checktype(L, arg);
size_t n = toflattable(L, arg+1);
size_t size = n * sizeoftype(type);
if(size == 0)
return luaL_argerror(L, arg+1, errstring(ERR_LENGTH));
ptr = (char*)AlignedAlloc(alignment, size);
if(!ptr)
return luaL_error(L, "failed to allocate page aligned memory");
err = testdata(L, type, n, ptr, size);
if(err)
{
free(ptr);
return luaL_argerror(L, arg+1, errstring(err));
}
CreateAllocated(L, ptr, size);
return 1;
}
int main(int argc, char **argv)
{
pointaddress();
sizeoftype();
point();
void_conversion();
ptr_vector();
DynMem();
// system("pause");
return 0;
}
void test_send(void)
{
up_command_t message;
message.mode = 5;
message.axes[0] = 1;
message.axes[1] = 2;
message.axes[2] = 3;
message.throttle = 50;
/* Pack a packet to send */
uint8_t txbuf[sizeof(osuar_packet_t)];
memset(txbuf, 0, sizeof(txbuf));
size_t msg_sz = sizeoftype(UP_CONFIG_TYPE);
size_t packet_sz;
protocol_pack(UP_CONFIG_TYPE, &message, txbuf, &packet_sz);
printf("Added %d bytes to buffer (message size %d, buffer size %d):\n\n", packet_sz, msg_sz, sizeof(txbuf));
/* Dump content of txbuf */
uint16_t i;
for (i=0; i<sizeof(txbuf); i++) {
printf("%02x", txbuf[i]);
}
printf("\n\n");
}
void test_receive(void)
{
uint8_t type;
/* Create ring buffer */
osuar_rb_t rxbuf;
uint8_t rxbuf_elems[50];
osuar_rb_init(&rxbuf, rxbuf_elems, sizeof(rxbuf_elems));
assert(rxbuf.size == 50);
assert(rxbuf.count == 0);
assert(rxbuf.head == 0);
/* Create incoming packets */
down_sync_t msg_sync;
msg_sync.gains[0] = 1;
msg_sync.gains[1] = 2;
msg_sync.gains[2] = 4.4;
msg_sync.gains[3] = 8.8;
msg_sync.gains[4] = 16;
msg_sync.gains[5] = 32;
msg_sync.trim[0] = -1.2;
msg_sync.trim[1] = 1.5;
/* The transmitter would send us this packet */
type = DOWN_SYNC_TYPE;
uint8_t txbuf[sizeof(osuar_packet_t)];
memset(txbuf, 0, sizeof(txbuf));
size_t msg_sz = sizeoftype(type);
size_t packet_sz;
uint16_t i;
protocol_pack(type, &msg_sync, txbuf, &packet_sz);
printf("Added %d bytes to TX buffer (message size %d, buffer size %d):\n\n", packet_sz, msg_sz, sizeof(txbuf));
printf("Contents of txbuf:\n\n");
for (i=0; i<sizeof(txbuf); i++) {
printf("%02x", txbuf[i]);
}
printf("\n\n");
/* Copy data to rxbuf */
assert(osuar_rb_add(&rxbuf, txbuf, packet_sz) == 0);
assert(rxbuf.count == packet_sz);
printf("Copied %d bytes from TX to RX buffer (buffer size %d).\n", packet_sz, rxbuf.size);
printf("Contents of rxbuf:\n\n");
for (i=0; i<rxbuf.size; i++) {
printf("%02x", rxbuf.elems[i]);
}
printf("\n\n");
/* Try to overfill buffer */
assert(osuar_rb_add(&rxbuf, txbuf, packet_sz) == 1);
/* Pull data from RX buffer */
uint8_t tmp[50];
assert(osuar_rb_remove(&rxbuf, tmp, packet_sz-2) == 0);
assert(rxbuf.head == packet_sz-2);
assert(rxbuf.count == 2);
/* This time, the buffer should have some room. */
assert(osuar_rb_add(&rxbuf, txbuf, packet_sz) == 0);
assert(rxbuf.head == packet_sz-2);
assert(rxbuf.count == packet_sz+2);
/* Attempt to get more than available. */
assert(osuar_rb_remove(&rxbuf, tmp, 30) == 0);
assert(rxbuf.head == (packet_sz+28) % rxbuf.size);
assert(rxbuf.count == (packet_sz-28) % rxbuf.size);
assert(osuar_rb_remove(&rxbuf, tmp, 30) == 1);
assert(rxbuf.head == (packet_sz+28) % rxbuf.size);
assert(rxbuf.count == (packet_sz-28) % rxbuf.size);
}
