Commit bcd3a834 authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Starting to implement receiving.

parent f51b1d8e
......@@ -3,6 +3,7 @@
#include <sysLib.h>
#include <string.h>
#include <taskLib.h>
#include <fppLib.h>
#include <stdlib.h>
#include <extctrl_pub.irc5_main_110310.h>
#include "extctrl2014_lc.h"
......@@ -84,9 +85,11 @@ static struct state {
unsigned char dst[6];
int channel;
int cookie;
int received;
struct encoder_writer {
struct labcomm2014_writer writer;
struct labcomm2014_writer_action_context action_context;
int unadjusted;
int need_ack;
int frag_start;
unsigned char buffer[BUFFER_SIZE];
......@@ -985,6 +988,7 @@ static int writer_start(
printf("%s %p %p\n", __FUNCTION__, signature, value);
w->pos = 0;
state.writer.unadjusted = 1;
if (signature != NULL && value != NULL) {
/* DATA packet */
state.writer.need_ack = 0;
......@@ -1025,21 +1029,25 @@ static int writer_end(
struct labcomm2014_writer_action_context *action_context)
{
int first, last, length, offset;
first = state.writer.frag_start + 10;
last = w->pos;
length = last - first;
w->pos = state.writer.frag_start;
labcomm2014_write_packed32(w, 0); /* frag_num */
labcomm2014_write_packed32(w, length); /* frag_length */
offset = first - w->pos;
w->data[last] = 'X';
memmove(&w->data[w->pos], &w->data[first], length);
last = last - offset;
w->data[last] = 'Y';
printf("%s %d %d %d %d\n", __FUNCTION__, first, last, length, offset);
hijacknet_send(state.driver, w->data, last + offset);
w->pos = 0;
if (state.writer.unadjusted) {
state.writer.unadjusted = 0;
first = state.writer.frag_start + 10;
last = w->pos;
length = last - first;
w->pos = state.writer.frag_start;
labcomm2014_write_packed32(w, 0); /* frag_num */
labcomm2014_write_packed32(w, length); /* frag_length */
offset = first - w->pos;
w->data[last] = 'X';
memmove(&w->data[w->pos], &w->data[first], length);
last = last - offset;
w->data[last] = 'Y';
printf("%s %d %d %d %d\n", __FUNCTION__, first, last, length, offset);
w->pos = last;
}
printf("%s %d\n", __FUNCTION__, w->pos);
hijacknet_send(state.driver, w->data, w->pos);
return w->error;
}
......@@ -1076,6 +1084,18 @@ static void init_writer()
state.writer.writer.pos = 0;
}
static int receive_hook(HIJACKNET_DRIVER *driver,
void *data, int length,
void *context)
{
FP_CONTEXT fp_context;
fppSave(&fp_context);
state.received += length;
fppRestore(&fp_context);
return length;
}
static int read_robot_data(char *path)
{
int result = 0;
......@@ -1258,12 +1278,18 @@ int hijacknet_extctrl2014_connect(char *interface,
hijacknet_extctrl2014_status();
init_writer();
hijacknet_add_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
receive_hook, NULL);
/* Send INIT */
labcomm2014_writer_start(&state.writer.writer,
state.writer.writer.action_context,
0, NULL, NULL);
labcomm2014_writer_end(&state.writer.writer,
state.writer.writer.action_context);
/* Immediate retry... */
labcomm2014_writer_end(&state.writer.writer,
state.writer.writer.action_context);
/* register data types */
state.encoder.encoder = labcomm2014_encoder_new(
......@@ -1275,7 +1301,10 @@ int hijacknet_extctrl2014_connect(char *interface,
state.encoder.encoder);
labcomm2014_encoder_register_extctrl2014_force_torque_net(
state.encoder.encoder);
taskDelay(1000);
labcomm2014_encoder_free(state.encoder.encoder);
hijacknet_del_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
receive_hook, NULL);
hijacknet_close(state.driver);
#if 0
#endif
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment