unsigned char
fob_errorcode(void)
{
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0x0a); // Error Code (FoBman p.119, p135, p149)
return sio_getc();
}
void
fob_status_print(void)
{
unsigned int retval1, retval2, retval, status;
int num, i;
// Bird Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0); // Bird Status (FoBman p.112)
retval1 = sio_getc();
retval2 = sio_getc();
status = ((retval2 & 0xff) << 8) | (retval1 & 0xff);
printf("status: %04x\n", status);
printf("master?: %c\n", (status & 0x8000) ? 'O' : 'X');
printf("init?: %c\n", (status & 0x4000) ? 'O' : 'X');
printf("error?: %c\n", (status & 0x2000) ? 'O' : 'X');
printf("running?: %c\n", (status & 0x1000) ? 'O' : 'X');
printf("hostsync?: %c\n", (status & 0x800) ? 'O' : 'X');
printf("expanded address mode?: %c\n", (status & 0x400) ? 'O' : 'X');
printf("crtSync?: %c\n", (status & 0x200) ? 'O' : 'X');
printf("noSync?: %c\n", (status & 0x100) ? 'O' : 'X');
printf("factory test?: %c\n", (status & 0x80) ? 'O' : 'X');
printf("XOFF?: %c\n", (status & 0x40) ? 'O' : 'X');
printf("sleep?: %c\n", (status & 0x20) ? 'O' : 'X');
printf("stream?: %c\n", (status & 0x1) ? 'O' : 'X');
// Flock System Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(36); // Flock System Status mode (FoBman p.130)
printf("Flock System Status (max %d birds)\n", fob_maxaddrs);
for (i = 0; i < fob_maxaddrs; ++i) {
retval = sio_getc();
printf("%3d: %s, %s, %s, %s, %s, %s, %s, %s\n", i,
(retval & 0x80) ? "ACCESSIBLE" : "not accessible",
(retval & 0x40) ? "RUNNING" : "not running",
(retval & 0x20) ? "a SENSOR exists" : "no sensors",
(retval & 0x10) ? "ERT" : "not ert",
(retval & 0x8) ? "ERT#3" : "no ert#3",
(retval & 0x4) ? "ERT#2" : "no ert#2",
(retval & 0x2) ? "ERT#1" : "no ert#1",
(retval & 0x1) ? "ERT#0 or SRT" : "no ert#0 nor srt");
}
}
int
fob_doit(float p[])
{
unsigned short s0;
//float p[16];
int c0, c1, i;
static int count = 0, c00 = 0;
// FoB manual p.40 Response Format
while (0 == (0x80 & (c0 = sio_getc()))) {
printf("[%02x] ", c0);
//putchar('.');
fflush(stdout);
};
c1 = sio_getc();
p[0] = fob_uchars2pos(c0, c1);
//printf("%02x %02x ", c0, c1);
for (i = 1; i < 3; ++i) {
c0 = sio_getc();
c1 = sio_getc();
p[i] = fob_uchars2pos(c0, c1);
//printf("%02x %02x ", c0, c1);
}
for (i = 3; i < 6; ++i) {
c0 = sio_getc();
c1 = sio_getc();
p[i] = fob_uchars2ang(c0, c1);
//printf("%02x %02x ", c0, c1);
}
c0 = sio_getc();
++count;
#if 0
if (count > 30) {
if (c00 >= c0) {
putchar(0x1b); putchar('8'); // restore cursor
//puts(" ");
loop_count();
putchar(0x1b); putchar('8'); // restore cursor
}
printf("(%d) %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f ",
c0, p[0], p[1], p[2], p[3], p[4], p[5]);
fflush(stdout);
if (count > 31) {
count = 0;
}
}
#endif
c00 = c0;
return c0;
}
/* This will read from the serial port until size characters have been read or
EOF (RX FIFO is empty). */
size_t sio_read(char *buf, size_t size)
{
char *p = buf;
size_t i;
int c;
for (i = 0; i < size; i++) {
if ((c = sio_getc()) == -1)
break;
p[i] = (char)c;
}
return i;
}
int
xgetc(int fn)
{
if (OPT_CHECK(RBX_NOINTR))
return (0);
for (;;) {
if (ioctrl & IO_KEYBOARD && getc(1))
return (fn ? 1 : getc(0));
if (ioctrl & IO_SERIAL && sio_ischar())
return (fn ? 1 : sio_getc());
if (fn)
return (0);
}
/* NOTREACHED */
}
void
fob_open(char *fn, speed_t speed, int usec)
{
int fd;
//char *fn = "/dev/ttyS0";
unsigned int retval1, retval2, retval, status;
int num, i;
char strbuf[1024], *str;
if (-1 == (fd = open(fn, O_RDONLY))) {
perror(fn);
exit(1);
}
rtsclear(fd);
close(fd);
usleep(usec);
fd = sio_init(fn, speed);
rtsclear(0);
// Bird Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0); // Bird Status (FoBman p.112)
retval1 = sio_getc();
retval2 = sio_getc();
status = ((retval2 & 0xff) << 8) | (retval1 & 0xff);
#if 0
printf("status: %04x\n", status);
printf("master?: %c\n", (status & 0x8000) ? 'O' : 'X');
printf("init?: %c\n", (status & 0x4000) ? 'O' : 'X');
printf("error?: %c\n", (status & 0x2000) ? 'O' : 'X');
printf("running?: %c\n", (status & 0x1000) ? 'O' : 'X');
printf("hostsync?: %c\n", (status & 0x800) ? 'O' : 'X');
printf("expanded address mode?: %c\n", (status & 0x400) ? 'O' : 'X');
printf("crtSync?: %c\n", (status & 0x200) ? 'O' : 'X');
printf("noSync?: %c\n", (status & 0x100) ? 'O' : 'X');
printf("factory test?: %c\n", (status & 0x80) ? 'O' : 'X');
printf("XOFF?: %c\n", (status & 0x40) ? 'O' : 'X');
printf("sleep?: %c\n", (status & 0x20) ? 'O' : 'X');
printf("stream?: %c\n", (status & 0x1) ? 'O' : 'X');
#endif
if (0 == (status & 0x8000)) {
fprintf(stderr, "FoB Hardware Config Error: not master\n");
exit(1);
}
// FBB Addressing mode
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(19); // FBB Addressing mode (FoBman p.123)
retval1 = sio_getc();
str = "Unknown";
switch (retval1) {
case 0:
str = "Normal";
num = 14;
break;
case 1:
str = "Expanded";
num = 30;
break;
case 3:
str = "SuperExpanded";
num = 126;
break;
default:
fprintf(stderr, "FATAL ERROR: unknown addressing mode\n");
exit(1);
break;
}
fob_addrmode = retval1;
fob_maxaddrs = num;
fob_num_sensors = 0;
printf("FBB addressing mode: %s (%x)\n", str, retval1);
// Flock System Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(36); // Flock System Status mode (FoBman p.130)
//printf("Flock System Status (max %d birds)\n", num);
for (i = 0; i < num; ++i) {
retval = sio_getc();
#if 0
printf("%3d: %s, %s, %s, %s, %s, %s, %s, %s\n", i,
(retval & 0x80) ? "ACCESSIBLE" : "not accessible",
(retval & 0x40) ? "RUNNING" : "not running",
(retval & 0x20) ? "a SENSOR exists" : "no sensors",
(retval & 0x10) ? "ERT" : "not ert",
(retval & 0x8) ? "ERT#3" : "no ert#3",
(retval & 0x4) ? "ERT#2" : "no ert#2",
(retval & 0x2) ? "ERT#1" : "no ert#1",
(retval & 0x1) ? "ERT#0 or SRT" : "no ert#0 nor srt");
#endif
if (retval & 0x80) {
++fob_num_birds;
}
if (retval & 0x20) {
fob_sensors[fob_num_sensors] = i + 1;
++fob_num_sensors;
}
}
#if 0
printf("fob_addrmode: %d\n", fob_addrmode);
printf("fob_maxaddrs: %d\n", fob_maxaddrs);
printf("fob_num_sensors: %d\n", fob_num_sensors);
for (i = 0; i < fob_num_sensors; ++i) {
printf("fob_sensors[%d]: %d\n", i, fob_sensors[i]);
}
#endif
printf("fob_num_birds: %d\n", fob_num_birds);
}
static void * sior_rpc_server(u32 funcno, void * data, int size) {
int res = 0, c;
size_t s;
char * p;
struct init_arguments_t * i;
switch(funcno) {
case SIOR_INIT:
i = (struct init_arguments_t *) data;
sio_init(i->baudrate, i->lcr_ueps, i->lcr_upen, i->lcr_usbl, i->lcr_umode);
break;
case SIOR_PUTC:
c = *((int *) data);
res = sio_putc(c);
break;
case SIOR_GETC:
res = sio_getc();
break;
case SIOR_GETCBLOCK:
res = sio_getc_block();
break;
case SIOR_WRITE:
p = *((char **) data) + IOP_MEM;
s = *(((size_t *) data) + 1);
DI();
ee_kmode_enter();
res = sio_write(p, s);
ee_kmode_exit();
EI();
break;
case SIOR_READ:
p = *((char **) data) + IOP_MEM;
s = *(((size_t *) data) + 1);
DI();
ee_kmode_enter();
res = sio_read(p, s);
ee_kmode_exit();
EI();
break;
case SIOR_PUTS:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
res = sio_puts(p);
ee_kmode_exit();
EI();
break;
case SIOR_PUTSN:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
res = sio_putsn(p);
ee_kmode_exit();
EI();
break;
case SIOR_GETS:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
(char*)res = sio_gets(p);
ee_kmode_exit();
EI();
break;
case SIOR_FLUSH:
sio_flush();
break;
}
*((int *) data) = res;
return data;
}
// Same as above, but blocking.
// Note that getc should be blocking by default. Ho well.
int sio_getc_block()
{
/* Do we have something in the RX FIFO? */
while (!(_lw(SIO_ISR) & 0xf00));
return sio_getc();
}
