Calypso Telescope Servo Controller Programmers Manual

Gary J. McGrath
February 18, 1998


Table of Contents


Preface

This document was created using Microsoft Word. Typeface usage is as follows:

ALL CAPS

All capital letters are used for filenames in the DOS operating system. Keys on the keyboard, such as ESC, are also depicted with all capital letters.

Italics

Explicit parts of the software are displayed in italics. These include:

  • Function names
  • Variable names
  • Variable types
  • C/C++ language constructs
  • TCC Commands
  • Constants created with #define

 

Note: Constants created with #define are usually all capital letters. In this case they will be depicted in italics with the same case as they are defined.

 


System Overview

The telescope servo control system consists of servo computers that control the position of the telescope axes. A master computer acts to coordinate and communicate with the position servo computers on a common bus. The processes that run on each computer CPU, have several tasks (or threads) running simultaneously which run under a system of interrupt-driven priority-based scheduling. This is a multiprocessing multitasking system.

Processes and CPUs

Each of the three servo CPUs runs its own servo process to control one of the telescope axes. There is also a master computer CPU that runs the master process. The master process monitors the operation of the servo CPUs and communicates information and commands to them. These CPUs are all connected to a common bus that permits messages and data to be sent from one CPU to another. The computer system manufacturer, Ziatech Corporation, calls this system of communication the Star System.

Communicating between CPUs

There are two mechanisms for communicating between CPUs. The first is inter-processor communication, which is a system of communication that allows a message to be sent to the message queue of another CPU. A message consists of an unsigned long word that will (hopefully) be understood by the process running on the receiving CPU. The Star System sets aside a block of shared memory to facilitate this mechanism.

The second mechanism uses blocks of shared memory, which are used by more than one CPU. The Shared memory is physically located on its own bus card. Each CPU defines an extern struct with the address of a specific block of this memory. In our case, this structure is a servo_block structure. Variables within these structures may be written to or read from by any CPU. This could be a very dangerous situation since data may be corrupted and no one will know where or how it was changed. This is why it is important to use shared memory blocks carefully. The software in the system should be written so that variables in shared memory are written to in as few places as possible (preferably one). Also, only one CPU should be allowed to write to a shared memory variable.

Task Switching

Within each process is a system of task scheduling that allows multiple tasks or threads to run simultaneously. These threads are not self contained, but operate out of the same address space and are able to share most of the same code and data. The scheduler switches between tasks by a combination of interrupt-driven and priority based context switching. VRTX is the real-time operating system that controls the task switching for each process.

The context of a task consists of the contents of the CPU flags and registers. When the scheduler switches tasks, it saves the general registers, program counter, coprocessor registers memory page registers, memory mapped I/O location mirror images, and special variables. These values are then loaded for the new task, which continues executing where it was the last time it was executing.

Each task in the system is assigned a priority that is used in the task scheduling procedure. The task with the highest priority that is capable of execution continues to execute until it suspends. This is cooperative multitasking because a task cannot be interrupted by another task, even if the task has a higher priority. A task must suspend itself. (A system in which tasks can be interrupted by higher priority tasks would be called preemptive-multitasking.) A task is not capable of execution if it suspends, terminates its own operation, or is deleted by another task. A task may suspend if it is waiting for a specific event. This may include waiting for a device to become ready, pending for a message from a mailbox or queue, or delaying for a specified period of time. For example, a task may suspend when a call to sc_tdelay() is made. At the end of the given delay period, the task again becomes ready to execute. VRTX is also capable of using "round-robin" or "time-slice" scheduling on tasks with the same priority. But, this capability is not used in our applications since an accurate time base is required.

Controller Hardware

Controller Bus

All processor boards and interface controller boards are mounted in a 24-slot card cage. This card cage contains a power supply and an STD32 bus that connects each of the card slots. The bus is compatible with either STD32 or STD80 cards. The first slot in the card cage is slot X, which is reserved for the ZT8939 arbiter board. This board controls the usage of the bus by the CPU boards. A CPU will ask the arbiter if it can gain control of the bus for access to peripherals such as shared memory, disk drives, etc. The arbiter will give control if another CPU is not using the bus.

Master Processor

The Master processor is a Ziatech ZT8902 single board computer. This is a 25 MHz 486 SX with 4 Mbytes of RAM. This board is configured as the permanent master of the system. The permanent master has write access to the hard and floppy disk drives. A temporary master would only have read access to these disks. The disk drives are used for development purposes only. The ZT8902 also has 1 Mbyte of sector erasable FLASH memory that stores a PROM-disk. Program files, data files, and DOS are stored on this PROM-disk. This PROM-disk is designated drive P:, and is read/write the same as a regular disk . The Star BIOS is also stored in FLASH, but separate from the PROM-disk.

Two cascaded 8259 interrupt controllers are used on the ZT8902 board. This is the same configuration used for the IBM PC AT. The ZT8902 also contains two 8254 programmable interval timer chips that have three timers in each. The standard PC timer tick (53 ms) is used for the VRTX timer tick in the master process. This is generated from the first timer chip. Information on the 8259 and 8254 can be found in the manuals for the ZT8902 as well as the "Peripheral Components" handbook from Intel. The 8250 UART is used for serial communications on the ZT8902. This is the same as the 16450 and is used in most IBM PCs. Data for this chip can be found in the "Interface: Line Drivers & Receivers Data Book" from National Semiconductor.

Servo Processors

Ziatech ZT8902 single board computers are used for each of the servo processors. These boards are configured as temporary masters. The ZT8902 is a 60 MHz 486 DX2 processor board. A numeric coprocessor is available in the processor chip.

This ZT8902 also contains 4 Mbyte ram, and a PROM-disk in FLASH memory.

The servo processors also contains 8259 interrupt controllers and 8254 programmable interval timer chips. The VRTX timer tick is generated from the second 8254 chip for the servo processes. As stated for the master processor, data for the 8259 and 8254 can be found in the "Peripheral Components" handbook from Intel. The ZT8902 also contains two serial channels that are implemented with a 16450/8250 compatible UART. This is the same chip that is used in most IBM PCs.

Shared Memory

A separate board is used for shared memory. The Ziatech ZT8825 board contains 64 Kbytes of Extended, byte-wide, battery-backed memory. A 1 Amp-hour lithium battery is used to retain the memory when the system is powered down. Typically, a ZT8825 with 512 Kbytes of static ram draws less than 5 mA at 25°C. This corresponds to more than 10 years, which makes this battery limited only by its shelf life.

The ZT8825 board uses 24-bit memory addressing that is capable of addressing 16 Mbytes of address space. All CPUs in the system have read/write access to shared memory. A shared memory block is set aside for global variables by making a far pointer to address segment CC00h and offset 0000h. Also, some of this memory is used by the Star System for interprocessor communications.

Memory Map

The following is the memory map for peripherals that use the STD32 bus for addressing. All addresses are given in Hexadecimal.

CC000 - CFFFF

ZT8825 available for global memory block (16 Kbytes)

C8000 - CBFFF

ZT8825 reserved for Star System (16 Kbytes)

A0000 - C7FFF

ZT8981 EGA Controller (160 Kbytes)

I/O Map

The I/0 map for peripherals on the STD32 bus is given below. All addresses are given in Hexadecimal.

EE68 - EE6B

ZT8825 Extended memory board registers

F000

ZT8961 Digital I/O interface

FFF0 - FFFF

RTI-1260 A/D converter

FCxx - FFxx

I/O EXP signal

0300 -

VL-1297 (primary mirror support, temp sensors)

0400 -

VL-1297 (servo diagnostics)

D000 - D00F

Azimuth Encoder Interface

D010 - D01F

Altitude Encoder Interface

D020 - D02F

Left Rotator Encoder Interface

D030 - D03F

Right Rotator Encoder Interface

 

ZT8841 Quad Serial Interface

03B0 - 03DF

ZT8981 EGA controller

46E8, 0102

ZT8981 EGA controller

0060, 0064, 0067

ZT8981 keyboard interface

01F0 - 01F7

ZT8953 IDE controller

03F6 - 03F7

ZT8953 IDE controller

001F, 0081 - 0083

ZT8950 floppy controller

0087, 0090, 0091

ZT8950 floppy controller

03F0, 03F7

ZT8950 floppy controller


The Servo Process

The servo process is the program that ultimately controls the motion the telescope. There is a servo process for each axis of the telescope; azimuth, elevation, and instrument rotator. Each of these processes runs on a separate CPU in the Ziatech rack. This executable program, SERVO.EXE, is stored in the flash PROM disk, and is executed automatically when the CPU boots. The servo process runs in a continuous loop reading position from the position encoder and outputting a torque signal to the drive amplifiers. The position command is calculated from the equation of a cubic spline trajectory. The coefficients of this equation as well as the start and end times come from the master process that runs on its own dedicated processor.

Tasks

There are several tasks that run simultaneously within the servo process. These tasks are controlled by the VRTX real-time operating system. They execute via interrupt-driven priority based scheduling. The following is a list of tasks in the servo process:

The master() Task

The master() task is the primary task of the system. It first begins executing upon startup of the VRTX operating system. The master() task then spawns off other tasks: keyboard_task(), message_task(), and servo_task().

The master() task is also responsible for executing commands from the master process that it receives via interprocessor communications. The master() task continually pends for a message from the SAMPLER_QUEUE. Each unsigned long message corresponds to a command. These commands include 1 (create servo task), 2 (delete servo task), 3 (update servo constants), 4 (initialize servo), 6 (startup procedure), 7 (enable encoder reference), and 8 (disable encoder reference).

The command, 1 (create servo task), begins the servo loop processing by creating the servo task. The command, 2 (delete servo task), stops servo processing by deleting the servo task, and zeros servo output by calling the function zero_out(). The command, 3 (update servo constants), updates the constant servo parameters with values from the servo_block in shared memory. These parameters include I/O scale factors and offsets. The command, 4 (initialize servo), does two things. First, it creates instances of I/O classes if it is the first time initialization. Second, it initializes dynamic servo parameters stored in the servo_block in shared memory. For example, it sets time_last_update to present_time(). The command, 7 (enable encoder reference) enables the interrupt for the reference position sensor so that when the reference is detected, the encoder hardware counter is initialized and the command position is set to the preset encoder reading to avoid a sudden large error signal. Finally, the command, 8 (disable encoder reference), disables the encoder reference position sensor interrupt so that it has no effect when the sensor is detected.

The keyboard_task()

The keyboard_task() periodically checks the keyboard buffer. If the ESC key has been pressed, the program is terminated.

The message_task()

The message_task() receives and processes messages in the form of trajectory coefficients from the master CPU. This is accomplished by periodically checking the traj_waiting flag in the servo_block in shared memory. The servo process uses the message_task() to receive trajectory paths in the form of cubic spline coefficients, start time, and end time. When a trajectory is received, the task suspends until the start time. Then, the coefficients and times are copied to variables of the servo_block in shared memory. This is where the servo process accesses the coefficients when it needs them at each servo update.

The servo_task()

This is the task that performs the processing of the sampled data servo. The servo_task() continually calls servo_update() every 1 ms.

In the servo_update() function, the encoders are read from the encoder interface module. The command position is then calculated from the parametric cubic trajectory spline using the current time. (The coefficients of the equation are variables in the servo block.) If there is no valid set of trajectory coefficients, the command position is obtained by integrating the current velocity with time, and the velocity command is continuously ramped down to zero. For the DRIFT command, the command velocity is held constant until another trajectory is received. A set of trajectory coefficients is considered invalid if the present time does not fall between the start time and the end time associated with the coefficients.

Alternatively, the system could be placed in constant velocity mode or analog input mode by the configuration file. In these cases the polynomial trajectory is ignored. In the constant velocity mode, integrating the constant velocity command generates the command position. For the case of the analog input mode, the velocity command is read in from the diagnostic analog I/O board. This velocity is then integrated to generate the position command.

The servo compensation and filtering is then applied and the drive signal is output through the D/A to drive amplifiers. A PID compensator is used to modify the dynamic properties of the servo. The proportional, integral, and derivative constants are variables in the servo_block, and are loaded from a data file by the master process. Four optional programmable digital notch filters are used to remove some of the effects of mechanical resonances in the system. The parameters of these filters are also loaded from the configuration file by the master process and stored in the servo_block.

Additional analog and digital diagnostic signals are also optionally outputted to aid in tuning and debugging the system.

Timer Tick

The timer tick is the interrupt that controls the synchronization of the multithreaded system. It is like the heartbeat of the system. The timer tick is used by VRTX to check mailboxes and queues periodically. This occurs when a task is suspended while pending for a message. VRTX also uses the timer tick when a task uses the sc_tdelay() function to suspend a for a period of time. In this case, the delay time is given in the number of timer ticks.

The timer tick is generated by the interrupt handler, timer_ISR(), which is called every time the interrupt vector 0x77 is generated. This is hardware interrupt IRQ15, which is generated by timer 0 of timer/counter 2 every hundred microseconds. Since a 1 kHz sample rate is desired for the servo, a timer tick of 1 ms is required. This is accomplished with the use of prestime_tick_count in the timer_ISR() interrupt handler. Each time the handler is called, the prestime_tick_count is incremented by one. When the prestime_tick_count reaches 10, a VRTX timer tick is generated with a call to the VRTX function ui_timer(). The second timer/counter chip is not available on standard PCs. Other methods of generating timer ticks for VRTX may have to be used on other types of processors.

The timer_ISR() interrupt handler is installed by the PERIODIC_config() function. The PERIODIC_config() function first initializes the timer/counter by writing a configuration word to the chip's control register. The configuration word is one byte that consists of the counter number (0), mode command (low byte followed by high byte), mode value (mode 2, rate generator), and data format (binary). The initial count is then written to the counter/timer. A count of 800 is used to generate a 100 microsecond periodic tick from the 8 MHz oscillator. After the counter/timer is initialized, the interrupt controller is initialized using the Ziatech Device Driver Package (DDP). The ISR is installed and the interrupt is enabled. The PERIODIC_config() function is called by main() before the VRTX system is started.

The remove_periodic() function disables the counter/timer interrupt in the interrupt controller. This makes the operation clean by leaving the system in its original state after program execution. The counter/timer is not reset to its original configuration because it is not necessary. This device is not used by either DOS or the system BIOS. It will be initialized each time the SERVO program runs.

Servo / Master Interface

Communication can be made between the servo and master processes by two mechanisms; interprocessor communication, and shared memory blocks. The following is a list of the communications that take place between the master and servo.

Commands are sent to the servo process by interprocessor communication. The command, 1 (create servo task), begins the servo loop processing by creating the servo task. The command, 2 (delete servo task), stops servo processing by deleting the servo task, and zeros servo output by calling the function zero_out(). The command, 3 (update servo constants), updates the constant servo parameters with values from the servo_block in shared memory. These parameters include I/O scale factors and offsets. The command, 4 (initialize servo), does two things. First, it creates instances of I/O classes if it is the first time initialization. Second, it initializes dynamic servo parameters stored in the servo_block in shared memory. For example, it sets time_last_update to present_time(). The command, 7 (enable encoder reference) enables the interrupt for the reference position sensor so that when the reference is detected, the encoder hardware counter is initialized and the command position is set to the preset encoder reading to avoid a sudden large error signal. Finally, the command, 8 (disable encoder reference), disables the encoder reference position sensor interrupt so that it has no effect when the sensor is detected.

The real-time clock of the servo CPU is set by the master through the use of shared memory. Also, dynamic servo parameters are communicated back to the master using shared memory. Servo constants are communicated to the SERVO process from MASTER via shared memory during initialization.

Real-Time Servo Control

The telescope axis controllers consist of digital position servo. Only a single-pole low-pass filter is used to smooth the quantized output command from the digital to analog converter (D/A). Here, the digital portion of the servo will be discussed. Figure 1 shows a simplified block diagram of the servo. The digital part is performed in the servo_update() function.

Figure 1

 

Position and position command are represented by q and qcmd, respectively. qerr is the error signal, which is the position command minus the actual position.

The PID compensator transfer function consists of proportional, integral, and derivative terms.

The integral term provides dc accuracy by increasing the system type by one. A small constant error signal could increase the compensator output quickly because the integrator continually sums the error signal. The integral term also reduces the effects of nonlinearities such as Dahl friction. This is important because bearing friction makes the telescope highly nonlinear. The derivative term acts to damp out the system and contribute to stability. The proportional term adds to the "stiffness" of the servo by increasing the output torque proportionally to the error signal.

In general, increasing any of the gains in a compensator will improve disturbance rejection and increase the bandwidth of the servo. Also, increasing gains generally reduces stability margins. The key is to get the right balance and proportions of gains in the system. This requires knowledge of the plant dynamics. The phase and gain characteristics of the plant should be determined experimentally since the mechanical structure is very complex and difficult to model accurately.

The servo tuning procedure should be followed when adjusting these constants.

Digital PID Compensation

The PID compensator is a digital implementation of the analog PID compensator. This filter has the following transfer function:

Where Kp is the proportional gain, Ki, is the integral gain, Kd is the derivative gain, and s is the Laplace operator.

It can be seen from "ER-78 Digital Implementation of PID Compensation" that the analog transfer function can be closely approximated by the following set of difference equations:

Where u(k) is the input, x1(k) is the output, and k is the sample number. X2 is the state variable that holds the value of the integral. If the error signal becomes large, or the position does not change quickly enough, the integrator may become extremely large. To prevent the integrator from becoming so large that it saturates the D/A converter, it is limited. One method of limiting the integrator is holding its value at the limit if it goes over. Another method is resetting it to zero when it reaches an absolute value greater than the limit. The resetting method is used here so that during transients such as turning the system on, the servo will be reset to a neutral value where the output torque is likely to be near zero.

The class pid_filter is used to implement the difference equations above. This class contains variables for the constants and state variables. The member function set_constants() sets the values of the constants after the object has been created. The member filter() takes as an argument the input signal, and performs the difference equations. The limit is applied, and the filter output is returned.

A slightly different form of the PID transfer function is used when reading the constants from the configuration file. The transfer function above is transformed to the following by dividing out each term by Kp:

G(s) = P_term ( 1 + I_term/s + D_term * s)

Where P_term = Kp, I_term = Ki/Kp, and D_term = Kd/Kp. This form of the transfer function is easier for the designer to use since the proportional coefficient can be used to adjust the overall gain of the transfer function. Also the desired filter breakpoint frequency can be used to determine the integral and derivative coefficients as follows:

Where Zr and Zi are the real and imaginary components of the transfer function zeros. These values derived from the transfer function are:

where z is the desired damping ratio of the compensator zeros (normally 0.5). And, w z is the desired break frequency of the compensator zeros (radians/sec).

 

Digital Notch Filters

The filter that is used is a digital approximation to the analog notch filter with the transfer function:

Where w n, z n, w d, and z d are the numerator and denominator natural frequency and damping factors. These can be determined by simulation to obtain the desired effect.

From "ER-79 Derivation and Simulation of Second-Order Digital Notch Filter Using Prewarping", the difference equation that can be used to approximate the analog transfer function is:

Where x is the input, y is the output, and the constants are calculated from the following equations.

The frequencies are prewarped from their original values to account for the discrete sampling. The transformation that is used is.

 

 

Servo Limits

During transients, some of the signals may become very large. To prevent overflow and signal saturation problems, limits are incorporated into the software. One limit is used to prevent the integral of position error from becoming too large. This limit is the variable limit1. Another limit, limit2, is used to restrict the analog velocity command signal to its maximum and minimum allowable levels. These limits, which are servo_block variables, are loaded from a data file.

Since the D/A converter has a range of +/- 10 volts, limit2 should be set to 10.0. A reasonable value for limit1 would limit the integral term to the overall limit. To accomplish this, limit1 should be set to limit2/(P_term*I_term).

Position Normalization

The Farrand and Heidenhain encoder interface electronics have been designed to accommodate the full +/-270 rotation of the Azimuth and Instrument Rotator axes. This eliminates the need to map encoder readings to more than one full revolution. Both position commands and encoder readings are generated within the range of +/-270 degrees.

Cubic Spline Coefficients

The coefficients of each cubic spline trajectory are calculated by the master process and sent to the servo process. The message_task() receives and processes messages in the form of trajectory coefficients from the master CPU. This is accomplished by periodically checking the traj_waiting flag in the servo_block in shared memory. The servo process uses the message_task() to receive trajectory paths in the form of cubic spline coefficients, start time, and end time. When a trajectory is received, the message_task() suspends until the start time. Then, the coefficients and times are copied to variables of the servo_block in shared memory. This is where the servo process accesses the coefficients when it needs them at each servo update.

The servo_task() uses the shared memory block directly. It has no knowledge that the variables in this block are changing, but simply uses the variables (changed or not) every time through the servo loop.

The servo loop checks to see if the trajectory coefficients are obsolete. This could happen if the queue runs out of trajectories. If the present time does not fall between the start time and the end time, the command position is obtained by integrating the current velocity with time, and the velocity command is continuously ramped down to zero. For the DRIFT command, the command velocity is held constant until another trajectory is received. A set of trajectory coefficients is considered invalid if the present time does not fall between the start time and the end time associated with the coefficients.

Alternatively, the system could be placed in constant velocity mode or analog input mode by the configuration file. In these cases the polynomial trajectory is ignored. In the constant velocity mode, integrating the constant velocity command generates the command position. For the case of the analog input mode, the velocity command is read in from the diagnostic analog I/O board. This velocity is then integrated to generate the position command.

 

Real-Time Clock and GPS Synchronization

In the servo process, a software real-time clock is used to generate the present time needed by the serproc() function when evaluating the cubic spline trajectory. This real-time clock (RTC) is implemented in the files PERIODIC.C, TIMESYNC.C and RTC.CPP.

The real-time clock keeps time accurately by synchronizing with a GPS (Global Positioning System) clock every second. This is accomplished by the timesync() interrupt handler that is called every time the GPS clock sends a once per second interrupt.

The state of the RTC is determined by a set of global variables:

int rtc_100_usec;

int rtc_hundred_usec_error;

int rtc_clear_usec;

long rtc_seconds;

int rtc_set;

long rtc_set_seconds;

int rtc_set_100_usec;

The value of the real-time clock variable rtc_100_usec is incremented (every 100 microseconds. This variable keeps track of the fractional seconds. The number of seconds is stored in the variable rtc_seconds. The GPS receiver triggers interrupt 0x72 (IRQ10) once every second. When this happens, the interrupt service routine (ISR) timesync() sets the flag rtc_clear_usec. Then, the next time the timer_ISR() handler executes, the flag is cleared, rtc_100_usec is cleared, and rtc_seconds is incremented. Keep in mind that the GPS interrupt has a higher priority than the timer tick interrupt. This means that the GPS could interrupt the timer tick. This is why a flag is used to signal the seconds rollover. The check for the flag in the timer_ISR() takes only one instruction. Therefore, the GPS clock cannot corrupt values of the RTC by changing only some of them. All variables are either updated or not (depending on the value of the flag).

Since the timer tick will not run at the exact same speed as the GPS clock, the seconds may roll over either before or after the rtc_100_usec variable reaches 10,000. In order to reduce the amount of discontinuity in the RTC, the function that reads the present time has the ability to approximate the accumulated error and adjust the output accordingly. The function present_time() is used to read the RTC. The variable timer_adjust is used to determine the amount of adjustment that is required. timer_adjust is the error per 100 microseconds that must be removed from the RTC. The larger the fractional second is the larger the adjustment is necessary. This is a linear relationship. The timer_adjust is the error between the GPS and the RTC for the last one second time period divided by the number of increments in the fractional second (10000). The hundred_usec_error is calculated in the GPS handler timesync().

It is important to realize that either the timer_ISR() or the timesync() ISR could interrupt present_time() while it is reading the RTC global variables. This could cause the values to be corrupted since some values would have changed and others not. To avoid this problem, the RTC global variables that are used present_time() are read twice. This ensures that an interrupt has not occurred while reading these parameters. If any of the parameters have changed, they are all read again twice.

The date and time of the real-time clock are also occasionally set by the MASTER with an accuracy of one second. This is initiated by a SET.TIME command from the TCC. When the master receives this command it stores it in the RTC variables in the servo block in shared memory. The rtc_set variable is set last. This indicates that all variables are ready to be read into the software RTC. The timer_ISR() checks this flag every time it executes. If the flag is set, the ISR resets it and sets the rtc_seconds to the nearest time to the values sent by MASTER.

 

Encoder Interface and Diagnostic I/O

Encoder Interface Functional Description

The encoders used for servo controller position feedback are Farrand encoders for azimuth and elevation axes and Heidenhain incremental encoders for the instrument rotators. In addition to the Farrand encoders, the azimuth and elevation axes also include Heidenhain encoders, that are available for diagnostic purposes and are not integral to servo control processing. A total of four encoder interface modules (EIM) reside in the Ziatech chassis. The servo controller CPU assigned to a particular axis reads position feedback data from the EIM assigned to that axis. A dedicated controller CPU is assigned azimuth and elevation axes, while rotator controller CPU reads from either the left or right interface module, depending upon which rotator axis is currently active. The interface module design is common across all servo axis positions enabling a single spare module to be substituted into any position.

The Encoder Interface Modules (EIM) are mapped into the Ziatech computer's I/O address space. Each module occupies a unique 16-location segment of the 16-bit I/O address space common to all processors. Address selection jumpers on the interface modules determine the base address for each module.

Some of the performance parameters are specified below:

The principle components of the EIM include three programmable logic devices (PLD's), 8-bit data register IC's, and differential line receivers for signals received from the encoders. One PLD contains the logic for Farrand encoder signal processing, another processes Heidenhain signals, and the third device contains the STD bus interface logic. A group of register IC's latches the digital word present at the output of the Farrand and Heidenhain PLD's. Each time a read cycle is initiated, the registers are strobed, thereby obtaining a "snapshot" of the current encoder state. Data from the interface modules is read once each servo sample interval.

The Farrand encoder (with local electronics) gives absolute position over 1/512 revolution with a resolution of 16 bits. At the boundary of each 1/512-revolution cycle, the 16-bit data word transitions between FFFF and 0000 hexadecimal. At this transition, a ripple carry pulse (RC) is generated and the sense of the direction signal is transferred via cable to the EIM in the Ziatech chassis. The 16-bit data is actually multiplexed 4 bits at a time. Four transfers are required for the full 16 bits. This transfer occurs in approximately 3 microseconds.

The Farrand interface PLD receives and processes the 4-bit data, the ripple-carry, and direction signals. Logic within the Farrand interface PLD reconstructs the 16-bit word and also accumulates the ripple-carry pulses generated at the boundary of each sector. The direction signal indicates the direction of rotation at the time of the RC pulse and, therefore, determines the sense of the accumulator update. The accumulator forms the high 10-bit field of the total 26-bit word that represents Farrand encoder position over the total range of 540 degrees. The Farrand encoder is, therefore, absolute over each sector and external logic incrementally accumulates the sector transitions to encode position over the full travel range. The Farrand data registers, latch the 26-bit word along with a logic zero level in the six most significant bits to bring the word up to a byte boundary, 32 bits total. During a read process, data is transferred from these registers sequentially in byte-wide format; four transfers are required to read the current state of the Farrand encoder.

The Heidenhain encoder provides incremental position feedback in the form of two quadrature clock signals, which are produced, when the encoder is moving. These clocks are phased 90 degrees apart and the phase relationship between the signals is determined by the direction of travel. Logic on the EIM receives the clock signals and interpolates by a factor of 4 to obtain the minimum measurement increment. With each transition of either clock, an accumulator is incremented or decremented depending upon the direction of travel. In this manner a 30-bit word is formed which represents Heidenhain encoder position over the 540-degree range. The Heidenhain data registers latch the 30-bit word along with a logic 0 level in the 2 most significant bits to form a 32-bit word. As with the Farrand encoder, 4 byte wide transfers are required to read the current state of the Heidenhain encoder.

Both sets of registers, Farrand and Heidenhain, are latched simultaneously with the reading of the first byte of Heidenhain data. This corresponds to a read operation from the base address of the board. Thereafter, the remaining bytes are read sequentially as successive read operations increment through the module's I/O address space. The order of the single byte read operations is therefore required to begin at the base address. An address map illustrating the address assignments of the EIM registers is given below.

Encoder interface module registers

Address

Register

base+0

Heidenhain data byte 0 (data latch)

base+1

Heidenhain data byte 1

base+2

Heidenhain data byte 2

base+3

Heidenhain data byte 3

base+4

Farrand data byte 0

base+5

Farrand data byte 1

base+6

Farrand data byte 2

base+7

Farrand data byte 3

base+0x0e

Command Register

base+0x0f

Status Register

 

In addition to the data registers, there are two auxiliary registers; control and status. The control register can be used to implement specific control actions on the board. This register has read/write access and is initialized to 00h on power up. Writing a logic 1 to a particular bit position (the bit is set) implements the control function. The status register has read only access and is used for monitoring and diagnostic purposes. The bit assignments of these registers are illustrated below.

Control Register bit Assignments

bit 0

Interrupt Clear

bit 1

Interrupt Enable

bit 2

Farrand Reference Enable

bit 3

Heidenhain Preset

bit 4

Heidenhain Reference Enable

bit 5

Busy

bit 6

Spare (not used)

bit 7

Delay Clear

 

Status Register bit Assignments

bit 0

Spare (not used)

bit 1

Spare (not used)

bit 2

Spare (not used)

bit 3

Spare (not used)

bit 4

Spare (not used)

bit 5

Spare (not used)

bit 6

Spare (not used)

bit 7

Interrupt

The STD bus Interface PLD performs address decoding and other control functions. Logic within this PLD generates the strobe pulses for data registers and controls the tri-state enable functions during data access. The STD Interface PLD decodes the position select inputs, JP1 on the EIM. These jumpers determine the EIM base address as illustrated below.

Typically, jumpers are placed between adjacent pins in JP1, and one jumper is left out. This open jumper determines the EIM base address as follows:

Jumper

EIM base address

Axis

1-8 open

D000h

Azimuth

2-7 open

D010h

Elevation

3-6 open

D020h

Left Instrument

4-5 open

D030h

Right Instrument

 

Encoder Interface Device Driver

The encoder interface device driver consists of the class encoders, which is defined in the files ENCODERS.H and ENCODERS.CPP.

The constructor function sets the EIM base address, installs the reference position ISR, initializes scale factors and offsets to default values, and sets the Delay Clear bit of the Control register to enable the Heidenhain accumulator.

The destructor function removes the reference position ISR.

The set_base_address() member function sets the base address of the encoder interface and disables encoder presets and interrupts.

The enable_preset() member function enables interrupts and enables the Farrand and Heidenhain reference position preset.

There are also member functions for setting and getting the values of the Farrand and Heidenhain scale factors and offsets.

The member function read() reads the Farrand and Heidenhain encoder data registers and applies the scale factors and offsets. The encoder readings are returned in the variables pointed to by the two arguments of the function.

The reference position interrupt service routine, encoder_ref_isr() is not a member of the class, but is installed by the member function setup_encoder_isr(). The ISR sets the flag reset_encoder_offset which signals the read() function next time it is called to reset the encoder offset to the reference position minus the reset position. This produces an encoder reading equal to the reference position. The Interrupt Clear bit of the Command register is toggled high then low to reset the interrupt line in the EIM. Next, the Interrupt Enable bit of the Command register is cleared to disable further interrupts. Finally, both encoder reference enable bits are cleared and EOI is sent to the master and slave interrupt controllers.

Diagnostic Analog I/O

The VersaLogic VL-1297 analog I/O board is used for diagnostic analog I/O. The vl1297 class is used as a device driver for this board. The device driver object for this board is created in the function Servo_Initialization(). The base address is hard-coded to 0x400. This board is shared by all of the servo CPUs. Therefore, care must be taken so that only one servo is configured to use the board at a time. Within the servo_update() function, there are several places where diagnostic input or output is optionally used. Flags, which are set by the configuration file, determine which of these options are enabled. For instance, if analog velocity input is enabled, the vcmd_source flag is set to 1 (velocity input), and the analog input is used as a velocity command signal for the servo. The flag diagnostic determines which if any signal is output on channel 0 of the VL-1297. The possible values of diagnostic are:

 

Diagnostic Digital I/O

The MASTER and each SERVO process use the ZT8961 digital I/O board. Specific bits are assigned to each SERVO process to measure the execution of the servo_task() and message_task(). See the document "ZT8961 Digital I/O Channel Specification" for more details.


The Master Process

The master process performs several important tasks. This executable program, MASTER.EXE, is stored in FLASH EEPROM, and is executed automatically when the CPU boots. Its primary responsibility is to coordinate and communicate with the servo CPUs. The master also receives and interprets asynchronous commands from the TCC (Telescope Control Computer) via RS-232. Serial communications are also conducted with the secondary controller. Trajectories are generated by the MASTER for each of the axes. The MASTER also reads the status of various temperature and pressure sensors and limit switches on the telescope. Also, the master is a very useful troubleshooting tool. It may be used to monitor incoming commands, and view the status of dynamic servo parameters in real-time.

Tasks

Several tasks run simultaneously within the master process. These tasks are controlled by the VRTX real-time operating system, and execute via interrupt-driven, priority based scheduling. The following is a list of tasks in the master process:

The master() Task

The master() task in the master process is different from that of the servo process, but it does have some similarities. As in the SERVO process, the master() task of the MASTER process is the primary task of the system. It first begins executing upon startup of the VRTX operating system. The master() task then spawns off the sub-tasks: panelrun(), MessageReceiver(), bellows_task(), tertiary_task(), and a trajectory_task() for each axis. The master() task also creates the queues that are necessary for trajectory generation. It then pends for the termination message, which may be sent by the panelrun() task to terminate the program.

The panelrun() Task

This is the task that controls the screen and user interaction. First, panelrun() reads the data files of servo constants and stores these constants in the appropriate servo_block in shared memory. Then it displays screen labels and constants. An infinite loop is then entered where dynamic servo parameters are continuously written to the screen, and keyboard input is read. The keyboard can be used to do any of the following:

Exit the MASTER program

ESC key

Initialize Servo

F1 key

Start Servo

F2 key

Halt Servo

F3 key

 Reset All

F4 key

Perform Startup Sequence

F5 key

Perform Shutdown Sequence

F6 key

Switch the Axis to be Monitored

F7 key

 Switch to Diagnostic Window

F8 key

 

The MessageReceiver() Task

This task receives one message at a time from the communications port. Every VRTX clock tick it checks to see if a message was received by calling the Ziatech Device Driver Package (DDP) function Ser8250RecvEomChk(). Once a full message is received, the DDP function Ser8250RecvChk() is called to see how many characters are in the receive buffer. Then, characters are read one at a time (for a maximum of count characters) until the end-of-message (EOM) character is received. When it receives an EOM character, the string is processed by calling ProcessCommand(). This is a generic reusable function that, in this case, sends the string to the command interpreter. If the character is printable, it is added to the string. If the character is EOM, the string is NULL terminated, sent to ProcessCommand(), and a new string is started.

The bellows_task()

The bellows_task() performs all of the analog I/O for the MASTER process. In addition to monitoring and controlling the primary mirror support bellows, it also reads the temperature sensors, and diagnostic motor/tachometer currents and voltages. The first thing the bellows_task() does is creates the classes for the VL-1297 and RTI-1260 analog I/O device drivers. The VL-1297 uses input averaging. The number of averages is set by the configuration file parameter, input_averages. The task then enters an infinite loop where it performs I/O and calculations once every 5 VRTX clock ticks. The bellows air pressure is calculated as a function of elevation angle, and the result is used to control the air pressure through the VL-1297.

The tertiary_task()

The tertiary_task() moves the tertiary mirror rotator to the desired position (left, center, or right). Commands are sent from the command interpreter to this task via the global variables tertiary_left_cmd, tertiary_center_cmd, and tertiary_right_cmd. Every 20 VRTX clock ticks, the zt8961 digital I/O is read to determine which position (if any) the tertiary is in. Then the direction bit is set and the drive bit is toggled low for the necessary amount of time. Once the tertiary is off the sector switch it will continue in the same direction until the next switch is encountered. If necessary, the axis will be commanded again until it reaches the desired position.

The Trajectory Tasks

There is one trajectory_task() function. This task is instantiated three times (one for each axis). A task pends its respective PVT_QUEUE for a pvt triplet. A pvt is a specific point in the axis trajectory specified by position, velocity and time. These pvt's are sent to the PVT_QUEUE by the command interpreter. When it receives a pvt, it accepts all offsets from its respective OFFSET_QUEUE. If valid offsets exist, it adds the offsets to the P of the pvt. An offset is valid if the time stamp is newer than the time stamp of the pvt. This is because we only want to offset the pvt's that were queued up at the time of the +MOVE TCC command. The cubic() function is called to calculate the cubic spline coefficients and send them to the respective servo CPU. Then, the function waits until the current time is greater than the last trajectory end time minus 0.5 seconds. A half-second is used to give the trajectory generator plenty of time to do its calculations and communicate the results before the next trajectory is needed by the servo. The time required consists of the time it takes to calculate the next trajectory, the interval between trajectory task loops, and the communication time. This number may be modified as needed. The cycle then repeats by pending for the next pvt.

The Command Interpreter

The command interpreter is called with a command string from the MessageReceiver() task that reads commands over the RS-232 connection to the TCC. Each command string is parsed immediately. An AXIS command will cause the local variable, axis, to change state. This variable will determine the axis that will be affected by future commands. The C library function strncmp() is used to compare command strings of a given length. When the command string matches a known command, the command is executed and a response is sent back to the TCC with a call to TransmitResponse(). The various forms of the MOVE command are executed by sending a pvt to the PVT_QUEUE of the specific axis. Similarly, when a form of the +MOVE command is received, an offset pvt is sent the OFFSET_QUEUE of the specific axis. The PVT_QUEUEs and OFFSET_QUEUEs all reside in the master CPU. The DRIFT command is executed by causing the servo to continue at its present velocity. The drift position and velocity are transmitted back to the TCC. There are other commands that perform functions such as setting real-time clocks, initializing servos and setting servo limits.

The Control Panel

It was found more manageable to write the software using common C library functions rather than a large powerful screen panel library. The simplicity of the user interface did not warrant the use of such a tool. In addition, use of the C library functions allows more direct control over the user interface without the hidden intricacies of a panel library. Here is a list of library functions that were used in panelrun() for console I/O:

clrscr()

Clears the screen using the current background color.

gotoxy()

Puts the cursor at the given screen coordinates.

cputs()

Writes a character string to the screen.

cprintf()

Writes a formatted string to the screen (similar to printf()).

textbackground()

Sets the current background color.

textcolor()

Sets the current text color.

kbhit()

Tests to see if a key was pressed on the keyboard.

getch()

Gets a character from the keyboard buffer.

setcursortype()

Sets the cursor type to _NOCURSOR or _NORMALCURSOR.

The panelrun() task writes the servo constants and dynamic parameters to the screen. If the user presses the ESC key, panelrun() sends the termination message to the master task to terminate the program. The user can press F1 to initialize the servo that is displayed on the screen. This sends an "init servo" message to the servo. The F2 key is used to start the servo that is displayed on the screen. A "create servo task" message is sent to the servo to begin the servo loop. To halt the displayed servo, the user can press F3. This will send the "halt servo" message to the servo, which will delete its servo task.

The F4 key is used to clear any hardware faults such as limit switches or over-speed conditions. First the servos are halted and initialized. Then, the servo position commands are set to the encoder positions to avoid a discontinuity when the servos are started. Next, the PVT buffers are cleared and the D/A converters are zeroed. Finally, any over-speed conditions are cleared by toggling RATECLR and SAF_RESET_NOT low then high. This function puts each axis in a state where they can be restarted by pressing F2.

The F5 and F6 keys perform the startup sequence and shutdown sequence respectively.

The panel can only display one axis at a time. Therefore, the F7 key is defined to switch the screen to monitor a different servo. This simply changes the value of the servo_block pointer to point to the next servo_block.

The control panel can be used for analysis and troubleshooting but is not necessary for normal operation since all necessary commands are supplied by the TCC. Any TCC commands that are received are written to the message line. This is the second to last line of the screen. Any errors or messages generated by the master process are written to the bottom line of the screen by the function panl_msg(). This is used instead of printf() to preserve the integrity of the screen.

Timer Tick

The VRTX timer tick is generated differently on the master CPU than it is on the servo CPU. Since a high-speed timer is not necessary for the MASTER, the standard PC timer interrupt is used. This is the 53 ms periodic interrupt generated on vector 0x1C. This software interrupt is generated by the system timer hardware interrupt (vector 0x08), which is driven by timer 0 of the standard PC 8254 counter/timer (first chip). Timer 0 has an initial count of 1000h and is driven by a 1.19318 MHz oscillator. This generates an interrupt with a frequency of approximately 18.2 Hz. It is recommended that interrupt 0x1C be used instead of the hardware interrupt 0x08 because DOS also uses 0x08 for other purposes. DOS has reserved software interrupt 0x1C for user programs. In addition, the system BIOS automatically initializes the ISR and interrupt controller for the hardware interrupt upon system startup. This makes using interrupt 0x1C a simple matter of installing an ISR for that vector.

The VRTX timer tick is installed in main() using getvect() (to save the original ISR) and setvect() (to install the new ISR). The ISR that is installed is tickservice(). The ISR simply calls the VRTX clock tick function ui_timer() and returns. Since this is a software interrupt, there is no need to send an end-of-interrupt (EOI) signal to the interrupt controller.

 

TCC Serial Communications

The 8250 UART chip is used to handle the asynchronous serial communications. This RS-232 connection is used to receive servo commands from the TCC.

Two possible devices could be configured to be used for the communications; either COM1 on the ZT8902, or COM3 on the ZT8841. This depends on which UART is connected to the fiber-optic interface assembly (FOIA). COM3 is currently configured as the TCC serial port. The file SERIAL.C contains the initialization software for the serial communications. In the function ASYNC_config(), the Ziatech Device Driver Package (DDP) is used to initialize the hardware. First, the 8254 counter/timer is initialized. Then the 8250 interrupt controllers are initialized. The slave interrupt controller is initialized on the ZT8841. The W1 jumper on the ZT8841 must be removed to make the most significant address nibble 8 for the UART and PIC. COM3 is then initialized using parameters 9600, N, 8, 1. Similarly, COM4 is initialized for use by the secondary mirror controller communications.

The MessageRedeiver() task in COMMAND.CPP receives TCC commands through the port defined by TCC_PORT. The DDP library is also used in this task to check for incoming messages and to read the messages from the communications buffer.

Secondary Serial Communications

Secondary controller commands are received by the MessagReceiver() task from the TCC. These commands originate from the TOI and pass through the TCC via the TALK command. Any command that begins with "SECONDARY" is assumed to be a secondary command and is passed to the secondary_command() function. This function receives secondary controller commands, and sends them to the secondary controller via RS-232. (The signal is subsequently converted to RS-485 and sent to the controller.) The status response from the controller is reformatted and returned as a response to the TCC (Then ultimately back to the TOI). Before sending a command to the controller, if the sec_done_flag is not set, an attempt is made to read the motion complete flag (b) from the controller. If the controller is still moving due to a previous command, the motion complete flag will not have been sent. If this happens, a response is made to the TCC stating that the command cannot be executed because the controller is busy. For more information see "Functional Description -Secondary Interface Protocol" for details of the secondary controller's communication protocol.

Primary Mirror Support System and Temperature Sensors

The bellows_task() controls the primary support bellows and reads the diagnostic signals from the load cell, pressure sensor, and temperature sensors. The bellows air pressure is calculated as a function of elevation angle. The primary mirror is always supported by an axial force equal and opposite to the component of the gravitational force on the mirror in the axial direction. The mirror is held in place radially by an adjustable belly band. The equation for the bellows air pressure is:

IPxducerPressure = (pm_W * cos(theta) + pm_B + pm_E * sin(theta)) / (24 * pm_A)

Where theta is the elevation angle and all other parameters are read in from MASTER.CFG.

Cubic Spline Path Generation

Various forms of the MOVE and +MOVE commands are sent asynchronously to the master process by the TCC. The master must calculate trajectories and communicate to the servo CPUs with precise timing.

Data Flow

The diagram below shows the data flow for the generation of cubic spline trajectories.

 

 

 

The interpreter may receive one of the four forms of the MOVE command. A trajectory must then be calculated and sent to the corresponding servo CPU. The four forms of this command are:

MOVE p v t

Move axis so that at time t the position is p and the velocity is v.

MOVE p v

Move axis to position p with a velocity of v as fast as possible.

MOVE p

Move axis to position p with zero velocity as fast as possible.

MOVE

Move axis to current position with zero velocity.

The interpreter calls the move() function. There a pvtT structure is dynamically allocated and filled with the given p, v, and t values. For the two cases where v is not given, it is assumed to be zero. For the case where p is not given, it is assumed to be the p of the previous pvt. If the t is not given, it is set to -1.0 in the pvt structure. This indicates that the end time of the trajectory must be calculated. The pvt also contains a time stamp to indicate when it was created. This value is filled with the time now from the servo_block. The pvt structure is sent to the pvt_queue of the given axis in the master CPU. This is done by sending the address of the structure.

The interpreter may also receive three forms of the +MOVE command from the TCC. These commands indicate that the pvt's that are queued up should be offset by the given values. These commands are:

+MOVE p v t

Offset p, v, and t.

+MOVE p v

Offset p and v.

+MOVE p

Offset p.

The interpreter calls the function plus_move(). The offset values p, v, and t are stored in the offset structure of type pvtT. Any values that are not given are set to zero. The offset structure is sent to the offset_queue of the given axis. This is also done by sending the address of the structure.

There is a trajectory generator task for each of the three axes. These tasks run on the master CPU and are all the exact same function, trajectory_task(). A trajectory task pends its respective pvt queue for a pvt. When it gets a pvt it accepts all offsets from its respective offset queue. If valid offsets exist, it adds the offsets to the p of the pvt. An offset is valid if its time stamp is newer than the time stamp of the pvt. This is because we only want to offset the pvt's that were queued up at the time the +MOVE command was received.

The trajectory generator task then calls cubic() to calculate a trajectory for (p1, v1, t1) and (p2, v2, t2). (p1, v1, t1) is the previous set of pvt's, and (p2, v2, t2) is the current set of pvt's. The cubic spline trajectory coefficients (a, b, c, and d) are then written to the shared memory servo_block of the given axis.

The trajectory generator task then waits until the current time is greater than the last trajectory end-time minus 0.5 seconds. A half-second is used to give the trajectory generator plenty of time to do its calculations before the next trajectory is needed by the servo. This number may be modified as needed. The trajectory generator task then begins its cycle again by pending for the next pvt.

In each servo CPU, there is a message_task() that continually checks for trajectories in the next_xxx variables in the servo_block and puts the cubic spline coefficients in the currently executing variables of the servo_block as they are needed by the servo_task().

Calculation of Trajectory Coefficients

It is desired to generate trajectory paths that match the maximum acceleration and velocity capabilities of the telescope while preserving smoothness within and between trajectories. The function we use is a cubic spline with position as a function of time:

p(t)=at3+bt2+ct+d

The time, t, is the clock time minus the interval start time. The coefficients, a, b, c, and d, define the path during a particular interval.

Matrix manipulation methods could be used to solve for these coefficients, but a closed form solution is used instead. A closed form, algebraic solution is more computationally efficient than matrix methods.

The algorithm that will be developed calculates these coefficients for the interval between (P1, V1, T1) and (P2, V2, T2). This algorithm is implemented in the function cubic(). The position, velocity, and time intervals are defined as P12=P2-P1, V12=V2-V1 and T12=T2-T1 respectively. Since T1=0, t is bounded by the interval from 0 to T12. Using these definitions, we can solve for the coefficients:

Note that P1, V1, and T1 indicate the previous pvt triplet, and P2, V2, and T2 indicate the current pvt triplet. Initially, since there is no previous pvt, the current position and velocity are used for P1 and V1. If not given in the MOVE command, P2 defaults to the current position, and V2 defaults to zero. For the cases where T2 is not given by the MOVE command, the time interval must be calculated. By differentiating the cubic spline equation twice, the acceleration curve is defined by:

 a(t)=6at+2b

This linear equation has a maximum at either t=0 or t=T12. These maxima are expressed using the appropriate endpoint values:

 

Each of these equations will give a different maximum acceleration for a given value of T12. The maximum allowable acceleration of the telescope axis is defined as amax. This is used to determine the optimum value of T12. Rewriting the maximum acceleration equations with amax , we have two quadratic equations in T12:

 

 

Solving these quadratic equations, we have:

 

The sign of amax is chosen to give a positive discriminant (contents of the radical) in both cases. Of the four roots, the largest positive root is used for T12.

For the special case when P12=0, the quadratic equations reduce to first order:

The greater value is used.

Knowing T12, the cubic spline coefficients can now be calculated.

When calculating the trajectory, maximum allowable velocity was not considered. We must now determine if this trajectory reaches the maximum telescope axis velocity, vmax . If it does, we must use three new trajectories instead. The following figure shows these trajectories; maximum acceleration, maximum velocity, and maximum deceleration.

 

We must assume that the magnitudes of the velocities at t1 and t4 are less than vmax. The times t1 and t4 correspond to the original end times T1 and T2, respectively. We will now determine if it is necessary to use these alternative trajectories. The following algorithm is implemented in the function slew(). By taking successive derivatives, the position velocity and acceleration profiles are:

 

Since, v(t) can only be a maximum at a(t)=0, maximum velocity occurs at:

 

If tmax is within the trajectory time interval, then there is a maximum velocity within this interval. This maximum velocity is:

 

If the magnitude of this velocity is greater than the maximum allowable velocity, vmax , then we must compute the three new trajectories.

First we compute the time interval t12 . At constant maximum acceleration:

Solving for t12 we have:

Similarly, for interval t34:

 

Now, we will compute the delta positions for the two constant acceleration segments. The general equation for position with constant acceleration is:

 

Substituting our boundary conditions, we have:

 

 

Since the overall delta position is known, the middle delta position can be calculated:

 p23 = p14 - p12 - p34

The time interval, t23, can now be calculated for constant vmax:

 

Now that we have position, velocity, and time for each interval endpoint, the trajectory coefficients can be calculated. For the intervals t12 and t34, the cubic spline reduces to a quadratic. The coefficients are:

for t12:

and for t34:

For the interval, t23 , the cubic spline reduces to a first order with the following coefficients:

for t23:

These algorithms are implemented in software by the functions cubic() and slew(). First, cubic() is called from within one of the three trajectory tasks. The previous and present pvt's, and the axis number are given as arguments. The function cubic() then calculates the cubic spline using the limit of maximum acceleration. The end time of the trajectory is calculated if it is not already given. The maximum velocity of the trajectory is checked against the maximum allowable velocity of the telescope axis. If the trajectory is within this limit, the coefficients are sent to the appropriate servo CPU, and the trajectory end time is returned. Otherwise, the slew() function is called to recalculate the trajectory using three new trajectories; maximum acceleration, maximum velocity, and maximum deceleration. These three sets of coefficients are then sent to the appropriate servo CPU. Then the end time of the third trajectory is returned to cubic() where it is then returned to the trajectory task.


Development Environment

Real-Time Operating System

The VRTX real-time executive is used in each program of the telescope servo system. It allows efficient CPU usage in real-time by controlling the task switching of a multitasking system. It also has mechanisms for managing memory allocation, communication between tasks, asynchronous I/O, and timing. VRTX is used with DOS, and is linked directly into the executable program. Some of the more commonly used VRTX functions are listed below.

 

Task Management

sc_tcreate()

Task Create

sc_tdelete()

Task Delete

sc_tsuspend()

Task Suspend

sc_tresume()

Task Resume

 

Memory Allocation

sc_gblock()

Get Memory Block

sc_rblock()

Release Memory Block

sc_pcreate()

Create Memory Partition

 

Communication

sc_post()

Post Message

sc_pend()

Pend for Message

sc_accept()

Accept Message

sc_qpost()

Post Message to Queue

sc_qpend()

Pend for Message from Queue

sc_qaccept()

Accept Message from Queue

sc_qcreate()

Create Message Queue

 

Interrupt Support

ui_exit()

Exit from Interrupt Handler

ui_enter()

Enter Interrupt Handler

 

Timing

sc_tdelay()

Task Delay

ui_timer()

Announce Timer Interrupt

sc_tslice()

Enable Round-Robin Scheduling

 

Asynchronous Communication

sc_getc()

Get Character

sc_putc()

Put Character

sc_waitc()

Wait for Character

ui_rxchr()

Received-Character Interrupt

ui_txrdy()

Transmit-Ready Interrupt

 

Initialization

vrtx_init()

 Initialize VRTX

vrtx_go()

Start Application Execution

 

Refer to the "VRTX/86 User's Guide" for more information. VRTX runs on any 8086 family processor.

Compilers & Tools

Most of the software for this project was written in C/C++. Some of the code is written in assembly language, and some of the third party software, such as device drivers, is written in C. Much of the C++ code is written using object oriented techniques. This is desirable for many reasons including reusability and ease of maintenance. It is recommended that future code be written this way whenever possible. It is necessary for the declarations of C functions to be enclosed within extern "C" { } so that the linker knows that these functions are not C++ functions.

Borland C++ version 3.1 has been used to develop the control software. The Borland DOS IDE (Integrated Development Environment) is used on the ZT8902 (486) CPU. The IDE requires at least 1 Mbyte of extended memory. Also, CPU1 must be used since it is the permanent master, which means that it is the only CPU with write access to the physical disk drives. The IDE includes an editor, C++ compiler, assembler, linker, and symbolic debugger.

Compile and Link Settings

Project files are used to compile and link the master and servo programs. These project files contain the locations of the source files as well as the settings for compiling and linking. The Options->Compiler->Code Generation->large memory model should be used for all programs so that far pointers are used for code and data. For the master process, the Options->Compiler->Advanced Code Generation->Floating Point Emulation should be used because there is no floating point unit in the master processor. Under the same menu, 80287/387 Floating Point should be used since the servo processors contain floating point units. Also under this menu, 80386 instruction set should be used for the master process and servo process. Generate Underbars should be used to be compatible with the libraries that are used. Fast Floating Point is also used for all programs. Under the menu Options->Compiler->Optimization Options, the default settings are used. If it is found in the future that Fastest Code or Smallest Code optimization is necessary, these options may be selected.

The menu Options->Directories contains the search paths that are used for compiling and linking. Under this menu, Include Directories should contain: D:\BORLANDC\INCLUDE;D:\DDP\INCLUDE;D:SERVO\MASTER;D:\SERVO; D:\VRTX\INCLUDE;D:\BOX

The Library Directories field should contain: D:\BORLANDC\LIB

The Output Directory field should contain C:\BIN. This is the directory that OBJ and EXE files will be written to. This keeps the source directories clean.

The Source Directories field is left blank. This allows the compiler to search only the current directory for source files (or directories given for individual source files in the project). The IDE (BC.EXE) should be run from the application source subdirectory of the program that is being developed. For example, the current directory should be D:\SERVO\MASTER when working on the master process. This is necessary since the project file references source files using relative directories. In addition, the project file in the current directory will automatically be loaded when the IDE starts up (unless there is more than one project file in the current directory).

Software Locations

The project file for each program is stored in the source file for that program. General source files, that are used for all programs are stored in the directory D:\SERVO. Header files for all master and servo programs are also stored in the D:\SERVO directory. Only application specific files are stored in the application source subdirectories. The Ziatech device driver package is located under the directory D:\DDP. The VRTX source and object files are stored under the directory D:\VRTX. The classes for the general-purpose device drivers are located in the following directories: D:\ZT8961, D:\DAC729, D:\ENCODERS, D:\VL1297, and D:\RTI1260. Source code for the general-purpose filter classes are located in the following directories: D:\PID, and D:\NOTCH. Also, the source and object files for the text box routines are stored in the directory D:\BOX. These routines are used for drawing screen windows or boxes in text mode. The source code should be backed up onto floppy disks regularly. At least two revisions should be kept in backup. The utility SWBACKUP.BAT can be used to automate the source backup process. This batch file can be found in the directory C:\UTIL.