Absolute Position Encoder Electronics Upgrade - Appendix B.3
February 18th, 2000 -
Bob Broilo
B.3 Encoder.c
/*
* encoder.c
* runs the encoder data reciever/buffer system for prototype
************************************************************************
* Copyright (C) 2002 Bob Broilo, National Radio Astronomy Observatory
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* Bob Broilo - bbroilo@nrao.edu
* PO Box 0, Socorro NM 87801
************************************************************************
* This program reads the VLA encoder data stream from
* the fiber receiver/buffers. The program combines the
* coarse and fine data from both azimuth and elevation
* encoders and sends the position information to the ACU.
* The program runs on a ZWorld BL1600 (Little Giant)
* controller with an XP8120 digital output board connected
* to the PLCBus and an 4x40 LCD display connected to
* the LCD Bus.
*
* This program assumes that we are using the old 20-bit
* ACU. If a more bits are desired, another PLCBus output
* board will need to be added, or a better comm protocol
* designed (serial!). Or just use the BL1600 as an ACU.
*
* the program may be expanded to take over the fiber
* receiver/buffer function - yeah right someday! They
* are very timing dependent... perhaps there is a better
* way.
*
* The routine which combines the coarse and fine words
* (combine) is based on Tony Poyner's code.
*
* The data from the fiber recieve buffers is in 8-bit parallel
* format with 2-bit handshaking. The data to the ACU is 2x20bit
* parallel with one hold bit.
*
* Bob Broilo
*/
#define VERSION "Data Converter Rev. 8/11/1999_BB "
#define VERSION2 " 'The Hootis' "
/*
* Required Libraries
*/
#use ezioplc.lib /* PLCBus stuff */
#use eziopbdv.lib /* more PLCBus stuff */
/*
* Constants
*/
/* LCD module defines */
#define LCD1 0x080 /* top LCD Control Register (/USER1) */
#define LCD2 0x150 /* bottom LCD Control Register (LCDX) */
#define LCD_CLEAR 0x01 /* Command to clear display, home cursor */
#define LCD_FUNC 0x3C /* Command 8 bit, 2 lines, 5x10 dots */
#define LCD_DISP 0x0C /* Command disp on, cursor on, blink */
#define LCD_HOME 0x02 /* Command home cursor */
#define LCD_TIMEOUT 50 /* LCD not responding */
#define LCD_FREQ 20 /* delay between LCD updates */
/* LCD Messages */
#define BLANKLINE " "
/* PLCBus defines */
#define POS_OUT 7*32 /* Address of position output board */
/* Constants */
#define AZEL_DISPLAY 0 /* display mode: az and el decimal */
#define AZ_DETAILS 1 /* display mode: az bits */
#define EL_DETAILS 2 /* display mode: el bits */
#define MAX_POS 16777215 /* maximum position possible, all 24 bits on */
#define LSBRES 720.0/33554432.0 /* angle of one LSB (720/2^25) */
#define BUF_DELAY 4 /* Data valid delay */
#define BUF_TIMEOUT 10 /* timeout waiting for data */
#define PLC_TIMEOUT 50 /* PLC not responding */
#define DSP_TIMEOUT 1000 /* time till display reverts */
#define MAXSTRING 81 /* 40x2 display has 80 char max */
#define ACU_BITS 20 /* number of bits the ACU expects */
#define AZ_OFFSET 3 /* number of LSBs to drop for azimuth */
#define EL_OFFSET 3 /* number of LSBs to drop for elevation */
#define TRUE 1 /* You can't handle the truth! */
#define FALSE 0 /* He would disguise lies with the truth, to attack */
/* us. The attack is psychological, powerful... */
/* I/O ports */
#define INENLO 0x110 /* 8-bit in buffer&kb IN00-IN07 H1.1-H1.10 */
#define AZ_RDY 9 /* az data ready in IN08 H1.9 */
#define EL_RDY 10 /* el data ready in IN09 H1.12 */
#define AZ_HOLD 11 /* acu data hold IN10 H1.11 */
#define EL_HOLD 12 /* acu data hold IN10 H1.11 */
#define AZ_BUFFER 0x120 /* select az buffer out OUT1 H2.3 */
#define EL_BUFFER 0x121 /* select el buffer out OUT2 H2.4 */
#define SWITCH 0x122 /* select keyboard out OUT3 H2.5 */
#define SYNC 0x123 /* sync out for testing OUT4 H2.6 */
#define OUTBYTE 0x130 /* el data out OUTB1-OUTB8 H2.13-H2.20 */
/*
* Functions
*/
void lcdwait(int); /* wait for LCD busy to clear */
void lcdinit(int); /* initialize display */
void lcdprint(int, char *); /* print string to LCD */
void lcdhome(int); /* home LCD cursor */
int readpos(int); /* read position from fiber buffer */
/* combine coarse/fine into position */
unsigned long combine(unsigned int fword, unsigned int cword);
void bits16(unsigned int);
void bits32(unsigned long);
void update_display(int);
void update_acu(void);
/*
* Globals
*/
char result[MAXSTRING];
struct posdata {
unsigned int coarse, fine; /* data words */
unsigned int cf_diff; /* difference btw coarse and fine overlap bits */
unsigned long position; /* digital position */
int timer; /* time counter */
int valid; /* data valid flag */
/* the following data is for display and is not continuously updated */
double angle; /* angle in degrees */
double degrees, minutes, seconds; /* angle in DMS */
};
struct posdata az; /* position data */
struct posdata el;
int data_valid; /* incoming data valid flag */
int lcd_valid; /* LCD diplay working OK */
unsigned int cf_diff; /* difference btw coarse and fine overlap bits */
/*
* send 20-bit position data to ACU
* very hardware dependent because PLCBus=32bits and ACU=2x20bits
*/
void update_acu(void)
{
int i, data;
if(az.valid) {
for(i=1;i<=ACU_BITS;i++) { /* 20 bits AZ PLCBus out 1-20 */
data=(int)(az.position >> (i+AZ_OFFSET)) & 1;
plcXP81Out(POS_OUT+(ACU_BITS-i), data);
}
}
if(el.valid) {
for(i=9;i<=ACU_BITS;i++) { /* 12 MSB EL out PLCBus 21-32 */
data=(int)(el.position >> (i+EL_OFFSET)) & 1;
plcXP81Out(POS_OUT+ACU_BITS+(ACU_BITS-i), data);
}
data=(int)((el.position >> (EL_OFFSET+1) & 0x0FF)); /* 8 LSB EL out H2, OUTB1-OUTB8 */
outport(OUTBYTE,data);
}
/* Debussy realized that a work of art, or an effort to create beauty, was */
/* always regarded by some people as a personal attack... - Art of Noise */
}
/*
* wait for LCD busy to clear
*/
void lcdwait(int display_address)
{
int lcd_timer;
lcd_timer=0;
lcd_valid=FALSE;
while((inport(display_address)&0x80) &&
(lcd_timer < LCD_TIMEOUT))
lcd_timer++;
if(lcd_timer < LCD_TIMEOUT) lcd_valid=TRUE;
}
/*
* Initialization for LCD module
*/
void lcdinit(int display_address)
{
lcdwait(display_address); /* wait for LCD not busy */
outport(display_address,LCD_CLEAR);
lcdwait(display_address);
outport(display_address,LCD_FUNC);
lcdwait(display_address);
outport(display_address,LCD_DISP);
}
/*
* Home cursor in display
*/
void lcdhome(int display_address)
{
lcdwait(display_address);
outport(display_address,LCD_HOME);
}
/*
* print string to LCD at current cursor location
*/
void lcdprint(int display_address, char *lcstring)
{
int indx;
for(indx=0;indx<strlen(lcstring);indx++)
{
lcdwait(display_address);
outport(display_address+1,lcstring[indx]);
}
}
/*
* read a byte from a fiber recieve buffer
*/
int readpos(int buffer_select)
{
int readval, i;
outport(buffer_select,1); /* signal ready to read */
for(i=0;i<BUF_DELAY;i++); /* wait for buffer data valid */
readval = inport(INENLO); /* read byte */
outport(buffer_select,0); /* signal byte read */
return(readval);
}
/*
* Convert the 16-bit coarse and fine words into one 24-bit
* number representing absolute angle from 0-720 degrees.
* The coarse resolver is only accurate to 12 bits so we
* don't use the low nibble.
* Coarse : XXXXXXXX XXXX1111
* Fine: XXXXXXXX XXXXXXXX
* MSB ^ ^^^ Overlap ^ LSB
* Assuming that the fine encoder is more accurate than the
* coarse and that there are only 4 bit transistions difference
* between the two, we adjust the coarse by adding or subtracting to
* match the fine, then combine the data.
* Based on code by Tony Poyner
*/
unsigned long combine(unsigned int fword, unsigned int cword)
{
int c3, f3, sign, x;
unsigned long combined;
cword = cword/0x10; /* ignore low nibble */
/* TEMP change for syncro */
cword = (cword>>1); /* REMOVE REMOVE REMOVE */
c3 = cword & 0x07; /* low 3 bits coarse */
f3 = fword/0x2000; /* high 3 bits fine */
x = c3-f3; /* how far off are we? */
cf_diff = abs(x); /* save the difference for display */
if (x>4) cword = cword + (8-x); /* assume cword behind */
else if ((x<=4)&&(x>=-4)) cword = cword - x; /* twiddle cword */
else /* if (x<-4) */ cword = cword - (8+x); /* assume cword ahead */
cword = cword / 0x08; /* use top three bits from fine */
combined = (long)cword*0x10000 + (long)fword;
return(combined);
}
/*
* Convert 16-bit number into binary string of 1s and 0s.
* put the string in result[]
*/
void bits16(unsigned int dataword)
{
int i;
for(i=0;i<16;i++) { /* 16 bits in word */
if((dataword<<i)&0x8000)
result[i]='1'; /* high = '1' */
else
result[i]='0'; /* lo = '0' */
}
result[16]='\0'; /* terminate string properly */
}
/*
* Convert 16-bit number (from coarse) into 12-item binary string
* of 1s and 0s. Put the string in result[]
*/
void bits12(unsigned int dataword)
{
int i;
for(i=0;i<12;i++) { /* 16 bits in word */
if((dataword<<i)&0x8000)
result[i]='1'; /* high = '1' */
else
result[i]='0'; /* lo = '0' */
}
result[12]='\0'; /* terminate string properly */
}
/*
* Convert 32-bit number into binary string of 1s and 0s.
* put the string in result[]
*/
void bits32(unsigned long dataword)
{
int i;
for(i=0;i<32;i++) { /* 16 bits in word */
if((dataword<<i)&0x80000000)
result[i]='1'; /* high = '1' */
else
result[i]='0'; /* lo = '0' */
}
result[32]='\0'; /* terminate string properly */
}
/*
* Convert 32-bit number into 25-item binary string of 1s and 0s.
* put the string in result[]
*/
void bits25(unsigned long dataword)
{
int i;
for(i=0;i<25;i++) { /* 25 bits in position */
if((dataword<<i+7)&0x80000000)
result[i]='1'; /* high = '1' */
else
result[i]='0'; /* lo = '0' */
}
result[25]='\0'; /* terminate string properly */
}
void update_display(int mode)
{
double degrees, minutes, seconds;
switch(mode) {
case AZEL_DISPLAY: /* Az and El decimal degrees, faults */
{
lcdhome(LCD1);
lcdhome(LCD2);
lcdprint(LCD1, VERSION); /* display version information */
lcdprint(LCD1, VERSION2);
if(az.valid) {
az.angle=(double)az.position * LSBRES;
sprintf(result, "AZ: %10.5f ", az.angle); }
else
sprintf(result, "AZ: invalid ");
lcdprint(LCD2,result);
if(el.valid) {
el.angle=(double)el.position * LSBRES;
sprintf(result, "EL: %10.5f ", el.angle);
}
else
sprintf(result, "EL: invalid ");
lcdprint(LCD2,result);
lcdprint(LCD2,BLANKLINE);
break;
}
case AZ_DETAILS: { /* AZ coarse, fine, combined, and D M S */
lcdhome(LCD1);
lcdhome(LCD2);
if(az.valid) {
bits12(az.coarse);
lcdprint(LCD1, "AZ Coarse ");
lcdprint(LCD1, result);
sprintf(result, " %1d", az.cf_diff);
lcdprint(LCD1, result);
lcdprint(LCD1, " Fine ");
bits16(az.fine);
lcdprint(LCD1, result);
lcdprint(LCD1, " ");
bits25(az.position);
lcdprint(LCD2, " Comb ");
lcdprint(LCD2, result);
lcdprint(LCD2, " ");
az.angle=(double)az.position * LSBRES;
minutes = (az.angle - floor(az.angle));
degrees = az.angle - minutes;
minutes = minutes * 60.0;
seconds = (minutes - floor(minutes));
minutes = minutes - seconds;
seconds = seconds * 60.0;
sprintf(result, "%10.5f %4.0f%3.0f%4.1f ", az.angle, degrees, minutes, seconds);
lcdprint(LCD2, result);
lcdprint(LCD2, " ");
}
else {
lcdprint(LCD1, "Az position data invalid ");
lcdprint(LCD1, BLANKLINE);
lcdprint(LCD2, "Not receiving data from the Az encoder ");
lcdprint(LCD2, BLANKLINE);
}
break;
}
case EL_DETAILS: { /* EL coarse, fine, combined, and D M S */
}
lcdhome(LCD1);
lcdhome(LCD2);
if(el.valid) {
bits12(el.coarse);
lcdprint(LCD1, "EL Coarse ");
lcdprint(LCD1, result);
sprintf(result, " %1d", el.cf_diff);
lcdprint(LCD1, result);
lcdprint(LCD1, " Fine ");
bits16(el.fine);
lcdprint(LCD1, result);
lcdprint(LCD1, " ");
bits25(el.position);
lcdprint(LCD2, " Comb ");
lcdprint(LCD2, result);
lcdprint(LCD2, " ");
el.angle=(double)el.position * LSBRES;
minutes = (el.angle - floor(el.angle));
degrees = el.angle - minutes;
minutes = minutes * 60.0;
seconds = (minutes - floor(minutes));
minutes = minutes - seconds;
seconds = seconds * 60.0;
sprintf(result, "%10.5f %4.0f%3.0f%4.1f ", el.angle, degrees, minutes, seconds);
lcdprint(LCD2, result);
lcdprint(LCD2, " ");
}
else {
lcdprint(LCD1, "El position data invalid ");
lcdprint(LCD1, BLANKLINE);
lcdprint(LCD2, "Not receiving data from the El encoder ");
lcdprint(LCD2, BLANKLINE);
}
break;
}
}
main()
{
unsigned int cshi, cslo, fnhi, fnlo; /* raw bytes from fiber */
int display_mode; /* what should be on LCD */
int display_timer; /* timeout to revert to default */
int i;
int lcd_count; /* count of loops before updating LCD */
int kbd; /* value read from keyboard/switches */
lcdinit(LCD1); /* Initialize LCD top */
lcdinit(LCD2); /* Initialize LCD bottom */
display_mode=AZEL_DISPLAY; /* set default display */
display_timer=0; /* init timers */
az.timer=0;
el.timer=0;
az.valid=FALSE;
el.valid=FALSE;
lcd_count=0;
/* Init PLCBus */
/* Read headers, set options */
/* Record power-up in log */
while(1) /* main program loop */
{
for(i=0;i<BUF_DELAY;i++);
if(up_digin(AZ_RDY)) /* if Az pos available */
{
fnlo = readpos(AZ_BUFFER); /* read four Az bytes */
fnhi = readpos(AZ_BUFFER);
cslo = readpos(AZ_BUFFER);
if(up_digin(AZ_RDY)) /* if line still high, so far so good */
{
cshi = readpos(AZ_BUFFER);
for(i=0;i<BUF_DELAY;i++);
if(!up_digin(AZ_RDY)) /* if line went low, all is well */
{
az.fine = fnlo+(fnhi*0x100); /* combine bytes to word */
az.coarse = cslo+(cshi*0x100);
az.position = MAX_POS - combine(az.fine, az.coarse); /* Az is reverse of El */
az.cf_diff = cf_diff; /* save coarse/fine diff */
az.timer = 0; /* reset az timer */
az.valid = TRUE;
}
}
/* else az.valid = FALSE; */
}
if(up_digin(EL_RDY)) /* if El pos available */
{
fnlo = readpos(EL_BUFFER); /* read four El bytes */
fnhi = readpos(EL_BUFFER);
cslo = readpos(EL_BUFFER);
if(up_digin(EL_RDY))
{
cshi = readpos(EL_BUFFER);
for(i=0;i<BUF_DELAY;i++);
if(!up_digin(EL_RDY)) /* if line went low, all is well */
{
el.fine = fnlo+(fnhi*0x100); /* combine bytes to word */
el.coarse = cslo+(cshi*0x100);
el.position = combine(el.fine, el.coarse);
el.cf_diff = cf_diff; /* save coarse/fine diff */
el.timer = 0; /* reset el timer */
el.valid = TRUE;
}
}
/* else el.valid = FALSE; */
}
/* read keyboard/switches */
outport(SWITCH,1); /* pull switch line */
for(i=0;i<BUF_DELAY;i++); /* wait for buffer data valid */
kbd=(~inport(INENLO)&0x07); /* read keys */
outport(SWITCH,0); /* release switch line */
if(kbd&1) display_mode=AZ_DETAILS;
if(kbd&2) display_mode=EL_DETAILS;
if(kbd==0) display_mode=AZEL_DISPLAY;
/* check timers, set flags, change offsets */
az.timer++; /* update timers */
el.timer++;
/* display_timer++; */
if (az.timer > BUF_TIMEOUT) { /* if az or el timeouts set invalid */
az.valid=FALSE;
az.timer=0;
}
if (el.timer > BUF_TIMEOUT) {
el.valid=FALSE;
el.timer=0;
}
if(display_timer > DSP_TIMEOUT) {
display_mode=AZEL_DISPLAY;
display_timer=0;
}
lcd_count++;
if(lcd_count > LCD_FREQ) {
update_display(display_mode);
lcd_count=0;
}
update_acu(); /* send position to ACU */
hitwd(); /* hit watchdog timer */
}
}