Starting to implement receiving.

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