diff --git a/mc/hijacknet_extctrl2014.c b/mc/hijacknet_extctrl2014.c index d07d701936ea4e36d8acde5ad511618b2ce2b376..ec55c9ad1524149896921ca1b4a9c8a45c09ffc8 100644 --- a/mc/hijacknet_extctrl2014.c +++ b/mc/hijacknet_extctrl2014.c @@ -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