Commit 90f4d99e authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Correct sequence wrap around for simulator, add intermediate VxWorks code.

parent 4f51c2bf
......@@ -44,7 +44,8 @@ acN.value[0] acN.value[1] acN.value[2] acN.value[3] idN
#define MAX_JOINT 128
#define MAX_ROBOT 4
#define MAX_FORCE_SENSORS 1
#define BUFFER_SIZE 1500
#define WRITE_BUFFER_SIZE 1500
#define READ_BUFFER_SIZE 8192
extern unsigned long int cpuFrequency;
......@@ -86,18 +87,26 @@ static struct state {
int channel;
int cookie;
int received;
struct encoder_writer {
struct {
struct labcomm2014_writer writer;
struct labcomm2014_writer_action_context action_context;
struct labcomm2014_encoder *encoder;
int sequence;
int unadjusted;
int need_ack;
int frag_start;
unsigned char buffer[BUFFER_SIZE];
unsigned char buffer[WRITE_BUFFER_SIZE];
} writer;
struct {
struct labcomm2014_encoder *encoder;
int sequence;
} encoder;
struct labcomm2014_reader reader;
struct labcomm2014_reader_action_context action_context;
SEM_ID sem;
int lpconsume;
unsigned char buffer[READ_BUFFER_SIZE];
unsigned int read_pos;
unsigned int write_pos;
unsigned int available;
} reader;
struct {
struct labcomm2014_decoder *decoder;
int sequence;
......@@ -956,8 +965,8 @@ static int writer_alloc(
{
printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
w->data = state.writer.buffer;
w->data_size = BUFFER_SIZE;
w->count = BUFFER_SIZE;
w->data_size = WRITE_BUFFER_SIZE;
w->count = WRITE_BUFFER_SIZE;
w->pos = 0;
return w->error;
......@@ -1016,7 +1025,7 @@ static int writer_start(
labcomm2014_write_byte(w, flags);
labcomm2014_write_packed32(w, state.channel);
labcomm2014_write_int(w, cookie);
labcomm2014_write_int(w, index);
labcomm2014_write_int(w, state.writer.sequence);
/* reserve maximum space for frag_num and frag_length */
state.writer.frag_start = w->pos;
w->pos += 10;
......@@ -1048,7 +1057,8 @@ static int writer_end(
}
printf("%s %d\n", __FUNCTION__, w->pos);
hijacknet_send(state.driver, w->data, w->pos);
taskDelay(1000);
return w->error;
}
......@@ -1079,23 +1089,177 @@ static void init_writer()
state.writer.writer.action_context = &state.writer.action_context;
state.writer.writer.memory = NULL;
state.writer.writer.data = state.writer.buffer;
state.writer.writer.data_size = BUFFER_SIZE;
state.writer.writer.count = BUFFER_SIZE;
state.writer.writer.data_size = WRITE_BUFFER_SIZE;
state.writer.writer.count = WRITE_BUFFER_SIZE;
state.writer.writer.pos = 0;
}
static int reader_fill(
struct labcomm2014_reader *r,
struct labcomm2014_reader_action_context *action_context)
{
int result = 0;
if (state.reader.available) {
unsigned int consume;
consume = state.reader.available;
if (r->pos == READ_BUFFER_SIZE) {
/* Handle wrap around */
r->pos = 0;
} else if (r->pos + consume > READ_BUFFER_SIZE) {
/* Packet wraps around, consume first part */
consume = READ_BUFFER_SIZE - r->pos;
}
r->count = r->pos + consume;
state.reader.available -= consume;
result = consume;
printf("Fill %d %d %d\n", consume, state.reader.available, r->pos);
} else {
r->count = 0;
r->error = -ENODATA;
printf("Fill error\n");
}
return result;
}
static const struct labcomm2014_reader_action reader_action = {
.alloc = NULL,
.free = NULL,
.start = NULL,
.fill = reader_fill,
.end = NULL,
.ioctl = NULL
};
static void init_reader()
{
state.reader.read_pos = 0;
state.reader.write_pos = 0;
state.reader.available = 0;
state.reader.action_context.next = NULL;
state.reader.action_context.action = &reader_action;
state.reader.action_context.context = &state.reader;
state.reader.reader.action_context = &state.reader.action_context;
state.reader.reader.memory = NULL;
state.reader.reader.data = state.reader.buffer;
state.reader.reader.data_size = READ_BUFFER_SIZE;
state.reader.reader.count = 0;
state.reader.reader.pos = 0;
}
static int receive_hook(HIJACKNET_DRIVER *driver,
void *data, int length,
void *context)
{
FP_CONTEXT fp_context;
unsigned int free, avail;
unsigned char *src = data;
fppSave(&fp_context);
state.received += length;
avail = (state.reader.write_pos - state.reader.read_pos) % READ_BUFFER_SIZE;
free = READ_BUFFER_SIZE - avail;
if (free >= length + 2) {
unsigned int i, pos;
pos = state.reader.write_pos;
state.reader.buffer[pos] = (length & 0xff00) >> 8;
pos = (pos + 1) % READ_BUFFER_SIZE;
state.reader.buffer[pos] = (length & 0xff);
pos = (pos + 1) % READ_BUFFER_SIZE;
for (i = 0 ; i < length ; i++) {
state.reader.buffer[pos] = src[i];
pos = (pos + 1) % READ_BUFFER_SIZE;
}
state.reader.write_pos = pos;
}
fppRestore(&fp_context);
return length;
}
static int lp_consume()
{
unsigned int avail;
while (1) {
semTake(state.reader.sem, 1000);
avail = (state.reader.write_pos - state.reader.read_pos) % READ_BUFFER_SIZE;
if (avail > 2) {
unsigned int length, pos, sane, i, channel, cookie, index;
unsigned short id;
unsigned char flags;
pos = state.reader.read_pos;
printf("read_pos=%d avail=%d\n", pos, avail);
/* Length of raw packet */
length = state.reader.buffer[pos] << 8;
pos = (pos + 1) % READ_BUFFER_SIZE;
printf("length=%d pos=%d\n", length, pos);
length |= state.reader.buffer[pos];
pos = (pos + 1) % READ_BUFFER_SIZE;
printf("length=%d pos=%d\n", length, pos);
/* Setup reader */
state.reader.available = length;
state.reader.reader.pos = pos;
labcomm2014_reader_fill(&state.reader.reader,
state.reader.reader.action_context);
/* Sanity check package */
sane = 1;
for (i = 0 ; i < 6 ; i++) {
unsigned char b;
b = labcomm2014_read_byte(&state.reader.reader);
sane &= (b == state.src[i]);
}
printf("sane=%d\n", sane);
for (i = 0 ; i < 6 ; i++) {
unsigned char b;
b = labcomm2014_read_byte(&state.reader.reader);
sane &= (b == state.dst[i]);
}
id = labcomm2014_read_short(&state.reader.reader);
sane &= (id == EXTCTRL2015_ETH_ID);
flags = labcomm2014_read_byte(&state.reader.reader);
channel = labcomm2014_read_packed32(&state.reader.reader);
sane &= (channel == state.channel);
cookie = labcomm2014_read_int(&state.reader.reader);
index = labcomm2014_read_int(&state.reader.reader);
printf("sane=%d flags=%02x channel=%d cookie=%x index=%u\n",
sane, flags, channel, cookie, index);
if (!sane) { goto out; }
if ((flags & flag_LAST_FRAG) == 0) {
printf("Implement defragmentation\n");
goto out;
}
if ((flags & flag_IS_ACK) != 0) {
if (index == state.ack) {
self.ack = 0;
NOTIFY;
}
if (index ==
/* Decode one package */
out:
/* Update read position */
state.reader.read_pos =
(state.reader.read_pos + 2 + length) % READ_BUFFER_SIZE;
}
printf("Available: %d\n", avail);
}
return 0;
}
static int read_robot_data(char *path)
{
int result = 0;
......@@ -1269,8 +1433,20 @@ int hijacknet_extctrl2014_connect(char *interface,
return -9;
}
state.cpu_MHz = cpuFrequency / 1000000;
state.mutex = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
state.sem = semBCreate(0, 0);
state.sem_communicate = semBCreate(0, 0);
state.reader.sem = semBCreate(0, 0);
/* Enumerate signatures, since no proper code init in VxWorks */
init_extctrl2014__signatures();
state.received = 0;
init_writer();
init_reader();
state.reader.lpconsume = taskSpawn("HNlpconsume", 254, 0, 20000, lp_consume,
0,0,0,0,0,0,0,0,0,0);
state.driver = hijacknet_open(interface);
hijacknet_get_mac_address(state.driver, state.src);
......@@ -1278,7 +1454,6 @@ int hijacknet_extctrl2014_connect(char *interface,
printf("%s", etoa(state.src));
hijacknet_extctrl2014_status();
init_writer();
hijacknet_add_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
receive_hook, NULL);
......@@ -1289,34 +1464,32 @@ int hijacknet_extctrl2014_connect(char *interface,
labcomm2014_writer_end(&state.writer.writer,
state.writer.writer.action_context);
/* Immediate retry... */
taskDelay(1000);
labcomm2014_writer_end(&state.writer.writer,
state.writer.writer.action_context);
/* register data types */
state.encoder.encoder = labcomm2014_encoder_new(
state.writer.encoder = labcomm2014_encoder_new(
&state.writer.writer,
labcomm2014_default_error_handler,
labcomm2014_default_memory,
labcomm2014_default_scheduler);
labcomm2014_encoder_register_extctrl2014_irb2ext_net(
state.encoder.encoder);
state.writer.encoder);
labcomm2014_encoder_register_extctrl2014_force_torque_net(
state.encoder.encoder);
taskDelay(1000);
labcomm2014_encoder_free(state.encoder.encoder);
state.writer.encoder);
taskDelay(10000);
labcomm2014_encoder_free(state.writer.encoder);
hijacknet_del_receive_hook(state.driver, EXTCTRL2015_ETH_ID,
receive_hook, NULL);
hijacknet_close(state.driver);
printf("Received: %d\n", state.received);
printf("%d %d\n", state.reader.write_pos, state.reader.read_pos);
taskDelete(state.reader.lpconsume);
#if 0
#endif
// state.decoder.decoder = labcomm2014_decoder_new(reader, &state);
// state.unit = unit;
state.cpu_MHz = cpuFrequency / 1000000;
state.mutex = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
state.sem = semBCreate(0, 0);
state.sem_communicate = semBCreate(0, 0);
#if 0
if (lth_net_add_hook(state.unit, hook, &state) != 0) {
......
......@@ -227,7 +227,9 @@ class ExtCtrl(object):
self.send_INIT()
elif self.robot:
self.await_INIT()
def next_index(self):
self.local_index = max(1, (self.local_index + 1) & 0xffffffff)
def recv_loop(self):
ack = None
......@@ -330,7 +332,7 @@ class ExtCtrl(object):
for i in self.retries(self.local_index):
for f in fragmenter.fragments(max_length=self.max_length):
self.ethernet.send(f.getvalue())
self.local_index += 1
self.next_index()
def send_INIT_ACK(self):
flags = kind_INIT | flag_IS_ACK
......@@ -364,7 +366,7 @@ class ExtCtrl(object):
for i in self.retries(self.local_index):
for f in fragmenter.fragments(max_length=self.max_length):
self.ethernet.send(f.getvalue())
self.local_index += 1
self.next_index()
def send_DATA(self, data):
flags = kind_DATA
......@@ -376,7 +378,7 @@ class ExtCtrl(object):
fragmenter.write(data)
for f in fragmenter.fragments(max_length=self.max_length):
self.ethernet.send(f.getvalue())
self.local_index += 1
self.next_index()
def writer(self):
class Writer:
......
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