SOFTWARE-BASED RESOLVER-TO-DIGITAL CONVERTER

Abstract: A software-based resolver-to-digital converter (RDC)(80) for computing an angle of rotation theta of gimbal-mounted instrumentation (16) in a gimbal system. The RDC (80) computes the angle of rotation of the instrumentation using parameter signals received from a resolver (32) in the system. The parameter signals, along with a resolver reference signal are input into a multiplexor (60) and are multiplexed. The signals are then fed to an analog-to-digital converter (38) and converted to digital form. The digital signals are then input into a processor (36) associated with the system. This processor, in addition to performing numerous system computations, is programmed through appropriate software to filter the digital signals received, compute the value for the angle of rotation theta using a Fourier transform and output the computed value to system circuitry (31) for processing to enhance system operation. The present invention eliminates the need for additional resolver-to-digital converter hardware components (40, 42, 44), as the converter (80) is implemented at the existing system processor (36).

Description: SOFTWARE-BASED RESOLVER-TO-DIGITAL CONVERTER BACKGROUND OF THE INVENTION 1. Technical Field The present invention relates generally to gimbal-based tracking systems, and more particularly to a software-based resolver-to-digital converter implemented with existing system hardware, thereby reducing system size and cost 2. Discussion In many gimbal-based tracking and surveillance systems, it is necessary to determine the angle of rotation of a gimbaled instrument with respect to its mount in order for the system to function accurately. For example, in a two axis azimuth- elevation gimbal system, the system must track both the position of the inner elevation gimbal with respect to the outer azimuth gimbal, and the position of the outer azimuth gimbal with respect to the base. In order to compute the relative positions of the elevation and azimuth gimbals, a device must measure relative angles between the two gimbals. A resolver is a common angular measurement device used for measuring these relative angles between the elevation and azimuth gimbals. A resolver exhibits excellent performance, is relatively low in cost, is small in size, and exhibits a high degree of reliability. Because of these characteristics, a resolver is ideal for use in precision pointing and stabilization systems. In spite of the above-mentioned desirable characteristics, a resolver suffers from a limitation: it does not output the gimbal angle of rotation, but rather the sine and cosine of the particular angle of rotation. Thus, external instrumentation is required to compute the angle from the resolver-generated sine and cosine measurements. SUBSTITUTE SHEET (M£26) The most common device for computing the angle of rotation from the resolver output is the resolver-to-digital converter integrated circuit (RDC). An RDC receives the sine and cosine analog outputs from the resolver, together with an analog reference signal, and produces a digital output that represents the angle of rotation. Commercially available RDCs exhibit several desirable features. First, an RDC produces a digital output. As gimbal systems increasingly employ digital control electronics, the digital output from the RDC functions as an analog-to-digital interface with these digital control electronics. Second, an RDC is flexible in that the RDC is usually compatible with many types of resolvers, including resolvers with different reference frequencies. Third, an RDC is usually tolerant of reference amplitude and frequency drift, along with resolver temperature changes. Fourth, an RDC is relatively fast and produces a digital angle of rotation with relatively small time delay. However, presently commercial RDCs also have several limitations. First, the cost of a typical RDC integrated circuit is high compared to other integrated circuits. As a result, the cost of a digital controller card using one or more RDCs is greatly increased. Second, an RDC consumes a considerable amount of physical space on a circuit board on which it is implemented. Typically, an RDC integrated circuit chip may range from 28 pins to as many as 44 pins. As increased emphasis is continually placed on reducing the number of circuit boards needed to implement control system electronics, the size of the RDC is becoming more and more of a critical factor. Third, commercially available RDCs are inflexible in that the RDCs can not be tailored to meet program-specific requirements. For example, the maximum reference frequency that can be handled by current generation RDCs is approximately 25 KHz. If a custom built resolver had a reference frequency of 40 KHz, a current generation RDC would be unable to determine the resolver angle. Thus, a custom made part would have to be manufactured, driving up the cost of the system considerably. What is needed then is a resolver-to-digital converter that exhibits all of the above-mentioned advantages associated with commercially available RDCs and that is relatively inexpensive to implement, requires a relatively small amount of circuit board space and is flexible so that it is capable of meeting program-specific requirements. UIE26 SUMMARY OF THE INVENTION In accordance with the teachings of the present invention, a software-based resolver-to-digital converter (RDC) is provided for use in computing the angle of rotation between gimbaled instruments in a gimbal-based system. The software-based RDC Gnds particular utility in military and airline surveillance, targeting and tracking systems in which position transducers such as resolvers are used to track the rotation of gimbal-mounted instrumentation. The present invention utilizes existing system hardware programmed to perform requisite computations. As a result, system cost and size is reduced. In the inventive approach, gimbal-mounted instrumentation is provided in a gimbal system. A resolver is used to measure parameters of an angle of rotation associated with the instrumentation as the instrumentation rotates on the gimbal. The resolver outputs the measured parameters as analog signals, along with an analog reference signal. Multiplexing means are used to multiplex the signals output from the resolver. An analog to digital converter then converts the multiplexed analog signals to digital signals. Digital processing means is programmed to compute the angle of rotation. The digital processing means includes three main components: filter means for receiving the multiplexed digital signals and for reducing unwanted noise and signal sensitivity; computing means for computing a value for the angle of rotation from the filter signals; and output means for outputting the computed angle of rotation to the system for processing to enhance system operation. The filter means and the computing and output means are coded into and implemented at the existing digital processing means, thereby eliminating the need for additional resolver-to-digital converter hardware components and reducing the cost of the system. BRIEF DESCRIPTION OF THE DRAWINGS Other objects and advantages of the invention will become apparent upon reading the following detailed description and upon reference to the drawings, in which: FIG. 1 is a perspective view of one type of environment in which the present invention may be implemented; FIG. 2 shows a gimbal unit on which sighting instrumentation is mounted for use in the environment of FIG. 1; FIG. 3 shows additional components of the gimbal unit of FIG. 2; FIG. 4 illustrates a display screen of the sighting unit of FIG. 1 showing a detected target scene; FIG. 5 is a block diagram of the gimbal sight unit system of FIG. 1; FIG. 6 is a top view of a circuit board on which the circuitry implementing a prior art resolver-to-digital converter is located; FIG.7 is a block diagram of the resolver-to-digital converter of the present invention implemented by existing system components; and FIG. 8 is a flow diagram indicating the method by which the present invention functions. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT The following description of the preferred embodiments is merely exemplary in nature and is in no way intended to limit the invention or its application or uses. Referring to the drawings, FIG. 1 illustrates a perspective view of an M1A2 tank at 10 in which a gimbal-mounted gunners primary sight (GPS) 12 is implemented. The GPS 12 is a visual tracking system that allows a tank gunner (shown in phantom at 13) inside the tank to scan for, sight and lock on to detected targets. A sight unit of the GPS 12 incorporates a gimbal unit, such as the one shown at 14 in FIG. 2, that includes gimbal mounted instrumentation indicated generally at 16 and associated circuitry shown and described in detail below. FIG. 3 shows additional components of the gimbal unit of FIG. 2 located behind system mirror 18. As a target scene is detected by the GPS, the scene is reflected off of mirror 18, and focused by focusing optics 19 onto a scene detector 20 for processing. The scene detector is preferably a gyro of the type and well known in the art and may be either infrared or visible spectrum light sensitive. The gyro is used by the control system to stabilize the mirror 18 so that the scene is still imaged without blur even in the presence of base motion of the tank, such as when the tank drives over rough terrain. As is known, gimbal units such as the one shown at 14 are often installed in vehicles or equipment in which hardware components such as resolvers are used as position transducers to enhance the accuracy of tracking, targeting or surveillance SUBSTTTUTESHEET(RUl£26) equipment. The gimbal mounted instrumentation 16 is capable of rotating freely to allow the gunner 13 to effectively track and lock onto the target and follow the target, shown at 22 on a system display screen 24 in FIG. 4, in a manner well known to those skilled in the art regardless of the direction of movement of the tank 10. Referring to FIG. 5, a block diagram of the GPS system 12 is shown. The system includes, in addition to gimbal mounted instrumentation 16 and display screen 24, gunner sight controls 26, a system power supply 28, a system clock 30, system circuitry 31 and a resolver 32. Gunner sight controls 26 allow the gunner 13 to manipulate the GPS 12 to locate and maintain a lock on the target 22. The system power supply 28 is remote from the instrumentation 16. Preferably, the vehicle in which the system is implemented supplies the power to the system. The system clock 30, preferably implemented in the system processor, is an integral part of the software-based timing routines of the present invention discussed below. As is shown in FIG. 5, the resolver 32 can be modeled as a transformer having a rotor winding and two stator windings. The resolver 32, of a type well known to those skilled in the art, electromechanically measures parameters received from the scene detectors 20 and associated with the angle of shaft rotation θ of the gimbal mounted instrumentation to stabilize the instrumentation as the GPS instrumentation 16 is slewed up, down left and/or right. The resolver 32 is excited by a reference voltage which may be generated either by the vehicle 10 or internally on an electronics board 34 shown in FIG. 6. The transformation of stator winding SI is cos θ, while the transformation of stator winding S2 is sin θ. As shown in the model, the resolver measures sin θ and cos θ values associated with a particular gimbal-mounted instrument. Typically, one resolver is utilized for each piece of gimbaled instrumentation. In a typical three axis system in which azimuth, elevation and coarse azimuth are gyro stabilized, a separate resolver is required to measure the relative angle of each gimbal axis. As the resolver 32 is limited to computing the sine and cosine values of θ for a gimbal-mounted instrument, additional computation is required to extract the value for θ from these resolver measured values. Referring to FIG. 6, a top view of a circuit board 34 is shown on which circuitry is implemented to compute the value of θ. As shown, a digital signal processor (DSP) 36 performs various computations associated with the GPS system. The processor shown is an Analog Devices 2100 16 bit Gxed point digital signal processor operating at 12.5 MHz. However, it should be understood that any commercially available digital signal processor may be implemented such as Texas Instruments TIC30, Motorola DSP 56001, Analog Devices ADSP 21000 and Motorola Model No. 96000 processors. It should also be understood that equivalent microprocessors, such as Motorola 68000 Series, Motorola Power PC or Intel X86 microprocessors may also be implemented in the system to perform system calculations and to implement the present invention, as will be described in detail below. Still referring to FIG. 6, an analog to digital converter 38 is also shown. This analog to digital convenor is a Datel Model No. ADS-106MC analog digital convenor. However, any commercially available M bit analog to digital convenor may be used. The analog to digital convenor converts analog signals output from the resolver 32 into digital signals. The digital signals in turn are then processed by the DSP 36. Also shown are commercially available hardware resolver to digital convenors (RDCs) 40, 42 and 44 that the present invention is designed to replace in a manner set forth in detail below. Turning now to FIG. 7, a block diagram of a preferred embodiment of the present invention is shown. As shown, the resolver 32 outputs signals corresponding to cos θ output lines 46. The resolver 32 also outputs a signal corresponding to sin θ output lines 48. In addition, the resolver reference signal inputs 50 are used as both inputs and outputs to the invention. Signals from output lines 46, 48 and 50 are input into a differential to single ended signal convenor 52. For low accuracy implementations where the resolver outputs are tied to system ground, the differential to single ended converter may be omitted. The convertor 52 converts the signals output from the resolver 32 to single ended signals, which are then output on lines 54, 56 and 58, respectively into a multiplexor 60. In addition, scaling may be done by the differential to single ended converter in order to scale the resolver signals to signal levels compatible to the multiplexor. The multiplexor multiplexes the signals input on lines 54, 56 and 58 and outputs the resulting multiplexed signals to the sample and hold amplifier 64. The sample and hold amplifier may be internal to an analog to digital converter (ADC) 68, or may be a separate integrated circuit. The ADC may be of the type discussed previously in conjunction with FIG. 5. The digital signal is then output from the ADC into the resolver to digital convertor of the present invention, shown generally at 80. SUBSTITUTE SHEET (RULI26) It should be understood at this point that the system in which the present invention is implemented typically includes a multiplexor as discussed above. If the system does not include a multiplexor, one may be added to the system for minimal cost and board space expense. All of the other above components in FIG. 7 are existing system components. Also, the signals output from the resolver 32 on lines 46, 48 and 50 are preferably converted from differential to single ended signals at the convertor 52. The conversion, while not always necessary, is preferably used for high performance applications where induced noise is to be kept to a minimum. It should also be understood each particular resolver input 46, 48 and 50 may require its own ADC to maximize the throughput, depending on the particular application. Still referring to Fig. 7, the resolver to digital convertor 80 of the present invention includes three main components: an angle of rotation determination block 82; a digital prefilter 84; and a digital post-filter 86. Each of the components 82, 84 and 86 is implemented at the digital signal processor 32 shown in FIG. 5, by programming the particular component functions into the processor in a manner well known to those skilled in the art, so that the processor carries out the particular function of each component 82, 84 and 86 as discussed now in greater detail. For example, Appendix A shows a preferred program written in assembly language for use with an Analog Devices 2100 processor that provides suitable instructions to the processor for carrying out these functions. The angle of rotation magnitude determination block 82 performs three important functions: 1) it extracts the magnitude of the angle of rotation θ; 2) it determines the quadrant and resulting sign of θ through computations in which the sine, cosine and reference outputs of the resolver are variables; and (3) it estimates the quadrature voltage for both the sine and cosine winding of the resolver. The FFT magnitude determination block 82 computes the magnitude of the angle θ as follows. Referring to the resolver circuit model shown in FIG. 3, sinusoidal reference voltage Y ref is applied across the resolver rotor winding. The TTTU1I SHEET RULE 26) sinusoidal reference voltage Y ref is related to the cosine and sine resolver outputs Y^,, and Y ri „ respectively by the following equations: y re cos(2πf re ft) ycos=Acos(2πf rc ft + φ)cos(θ) ♦ q c «n(2πf re ft + φ) + V Crβ ( t ) y,in«Acos(2πf re ft + φ)sin(θ) + q s sin(2πf re t + φ) ♦ V SreJ ( t ) where F ref is the frequency of the reference drive, φ is the phase shift due to the RL combinations in the rotor circuit, q c is the quadrature magnitude for the cosine winding, q_ is the quadrature magnitude for the sine winding, V^ (t) and V Srej (t) are residual voltages often present that corrupt the signal output, and A is a constant. These residual components do not contain any fundamental components. A Fast Fourier Transformation (FFT) function is programmed into the processor 36. As is well known, if a time sequence h(t) is sampled N times at a sampling frequency of f s hertz, the formula for an N point FFT is: H(k) = Y a aO hdiΔ-De J 2 *^ for k = 0. 1.... N-l where Δ T = 1/f, and the sampling frequency f, is an integer multiple M of the reference frequency f ref . If N=M, the reference frequency component of the FFT becomes: HCW = h(0) + h(AT)e-> + ... hUn-m-Dϊ' *-*** = •■0 h(nΔT)cos(2πn/N) + j i X«0 h(nΔT)sin(2πn/N) = H R (£ ref ) + j H l (f ref ) where the FFT is expressed in its real and imaginary components. This equation, when expressed in magnitude and phase forms, is as follows: HCU - v W H iM' * m τ &) SUBSTTΠΓTE SHEET (RULE 26) When this FFT equation is applied to the sine and cosine equations associated with the model resolver above, the following, given in magnitude phase form, result: In the above equation, Y coβ (f ref ) and Y,..(f ref ) represent the value of the FFT of the cosine and sine at the fundamental frequency. Also, φ cloek represent the phase of the cosine and sine signals with respect to the computer clock, or the first sample of the sine and cosine, and φ c< , ωd and φ^ j represents the phase change due to the cosine and sine quadrature voltages, respectively. Assuming the quadrature voltages q c and q, are known, the magnitude of Acos(θ) anci Asin(θ) is determined by the following: I AsiiKΘ) P - CY^f.r f )) 2 - (qj 2 lAco^ l^ fY^f^ - lqJ 2 The magnitude of θ is thus determined by the following equation: In addition to the magnitude of θ. the resulting sign and quadrant location of θ must also be calculated in order to properly control system functions. In determining the sign and the quadrant location of the angle of rotation, there is a non zero phase shift φ between the reference and sine and cosine signals. Assuming φ is known, a computer software reference Y,,^ is computed by the following equation: BI26 This reference is in phase with the sine and cosine signals and can be used to demodulate the angle θ through the following equations: J o »ft B f«)yco.<t)<it "" cos(2*kt + φ) ( Acos(2it^ f t + φ)cos(θ) + ■ i q c sin(2τc f t + φ) + V^d)} dt _ Acos(θ) , o , Q _ Acos(θ) = VI ~ VI y ofrc y i n Wdt s - Thus, once the signs of cos θ and sin θ are known, the quadrant in which θ is located is determined as computed by the following chart: Referring now to computation of the quadrature voltage, this voltage has the same frequency as the fundamental voltage, but is 90° out of phase with the fundamental frequency. The FFT magnitude determination block 82 which is a rectangular windowing function, is programmed to determine the quadrature voltage by measuring resolver stator voltages as the resolver rotates through a 0° fixed reference point. As sin θ should equal 0 at the 0° reference, any voltage measured at the 0° reference point is tracked out of the angle of rotation calculation to compensate for the quadrature voltage during system calculations. This calculation may need be calculated only once at startup, or can be repeated periodically to compensate for thermal effects. Referring now to the other components of the present invention as shown in FIG. 7, the digital prefilter 84 initially receives the digital input signals. The digital prefilter is a function programmed or coded into the digital signal processor and essentially acts as a filter around the reference frequency. Well known types of band pass filters, such as a Hanning window, a Blackman window, a rectangular windowing function or an analog band pass filter such as Chebyshev filter or a Butterworth filter SUBSTITUTE SHEET (RUlf 26) may also be used. The essential function of the filter 84 is to eliminate noise common in such a system, as the system is frequently implemented in vehicles such as tanks, airplanes and other similar environments in which a significant amount of electrical and electromechanical interference is present The digital post-filter 86 may be implemented in the system to further reduce noise associated with the system. A lowpass filter is preferably used to filter the computed θ signal before it is output. A lowpass filter is used because the output of the FFT magnitude determination block 82 is of a magnitude that may be represented as a DC quantity. Preferably, this digital filter 86 is an averager. However, it should be appreciated that other low pass filters such as Butterworth or Chebyshev filters could also be implemented through programming of the processor 36 to perform this particular function. FIG. 8 is a flow diagram, shown generally at 100, illustrating the operation of the system in which the resolver-to-digital converter of the present invention is implemented. At step 102, the resolver 32 computes parameters associated with θ upon receiving signals from gimbal unit 14. At step 104, the resolver outputs the angular parameters along with a resolver reference signal. At step 106, the differential signals output from the resolver are converted to single ended signals by the converter 52. At step 108, if the input digital signals are not input at the required system voltage, the signal voltage is also scaled appropriately by the sample and hold amplifier 64 at step 110. Next, at step 112, the signals are multiplexed by multiplexor 60. At step 114, the multiplexed, single ended analog signals are converted to digital signals by the ADC 68. The signals are then input into the processor 36 which is programmed to perform the requisite resolver-to-digital converter functions. At step 116, the signals are then filtered through filter 84 to eliminate noise and reduce signal sensitivity. At step 118, the resolver-to-digital converter of the present invention computes θ at determination block 82. At step 120, the present invention filters the computed angle θ at digital post-filter 86 to further reduce noise and enhance the accuracy of computation. At step 122, the resolver-to-digital converter of the present invention outputs the computed value for θ to the system to enhance system stabilization and system accuracy. At step 124, the system determines whether the application is complete. If the application is not complete, the system repeats the method at a rate SUBSTITUTE SHEET RUIE26 that is set corresponding to the input reference frequency. If the application is complete, the method ends. By implementing the resolver-to-digital converter 80 at the digital signal processor 36, the cost of the system in which it is implemented is greatly reduced by eliminating the need for separate additional RDC integrated circuit chips. Further, as the number of requisite hardware components is reduced, thereby saving a large amount of space on the board 34 is saved, thus reducing the requisite size of the board 34. It should also be appreciated that the software based resolver-to-digital converter 80 may be easily modified to operate at a desired particular frequency through minor software programming changes. This offers an advantage over present hardware based resolver-to-digital converters, as the hardware-based resolver-to-digital converters typically are designed to operate over a specific frequency range. Further, by implementing the software based resolver-to-digital converter 80, the tracking rate of the gimbal system, indicating the maximum gimbal rate that can be achieved without the resolver-to-digital converter losing lock, can exceed 1000° per second, as compared to a typical rate for a hardware resolver at digital converter of about 120° per second. It should be understood that the present inventions may be implemented in any system using resolvers or synchros, such as robotics systems, motor control systems, process control systems, systems that numerically control machine tools, and axis control systems. The present invention may be used in conjunction with a digital control system incorporating an analog to digital converter. The present invention is particularly suited for implementation in environments such as Korean EOTS, ALARM, IRST and Photonic Mast military systems. However, use of the present invention is contemplated for any land, air, sea or space-based tracking, targeting or surveillance system in which position transducers are implemented. Various other advantages of the present invention will become apparent to those skilled in the art after having the benefit of studying the foregoing text and drawings, taken in conjunction with the following claims. APPENDIX A ODU E/ROM/ABS=0 I AD_SW_RDC; ADSP 2100 Software RDC Algorithm Code Author: John Anagnost/ . H. Kieffer 8/02/94 Test version to generate 400 Hz square wave 9/12/94 MHK Added code to prevent sampling reference until it has settled. MHK 9/30/94 . var/dm/ram RefSampleDone; ( } .var/dm/ram ControlWord; { Copy of data last written to } . var/dm/ram SqrtlterCount; . var/dm/ram RefCmdToggle; . var/dm/ram RDCAzPosn; . var/dm/ram PreSignQQ; . var/dm/ ram YCos[10] ; . var/dm/ ram YSintlO] ; . var/dm/ram RefSettleDelayCoun ; i Lookup Tables .var/pm/rom RefCos[10]; ( .var/p /rom RefSin[10]; { .var/pm/rom CosLookup[10] ; { .var/pm/rom SinLookup[10] ; { .init RefCos: h#7FFF00,h#678Ξ00,h#278E00,h#D87200,h#987200, h#800000,h#987200,h#D87200,h#278E00,h#678E00 . init RefSin: h#000000, *4B3D00,h#79BC00,h#79BC00,h#4B3D00 h#000000,h#B4C300,h#864400,h#864400,h#B4C300 . init CosLookup: h#OCCDOO,h#0A5BO0,h#03F5O0,h#FCOBO0,h#F5A5O0 h#F33300,h*F5A500,h#FC0B00,h#03F500,h#0A5B00 . init SinLookup: h#000000, h#078b00,hιtOC2C00,h#OC2COO,h#078600 h#000000,h#F87AOO,hltF3D400,h#F3D400,httF87AOO .global ElDriftCmd; .global Inpu Port; .global Outpu Port; .global StartADC; .global AnalogSample; .global AzTorqSet; .global AzTorqCmd; .global ElTorqSet; .global ElTorqCmd; .global MirrXTorqSet; .global MirrXTorqCiπd; .global MirrYTorqSe ; .global MirrYTorqCmd; .global AzPosnDC; .global ElPosnDC; .global YRefReal; .global YRefliπag; ■global PhaseClockToRef; .global PhaseRotorToStator; .global CosTotalPhase; .global SinTotalPhase; .global YRef; .global CosRef; .global YCosReal , YCosImag; .global YSinRea l , YSinlmag ; .global PyyCoε , PyySin ,* .global QQ ; .global CosSu , SinSum; .global YCos, YSin; .global Sa-npCount; .global RefSampleDone; .global ControlWord; Copy of data last written to output port .global SqrtlterCount; .global RefCmdToggle; .global PreSignQQ; .global RefCos, RefSin; .global CosLookup, SinLookup; .global RefSettleDelayCount; { ■ END DECLARATIONS .newpage; INTERRUPT VECTORS rti; { Interrupt 0 is not used rti; ( Interrupt 1 is not used SUBSTITUTE SHEET (RUlf 26) jump ProcessCntllRQ; ( Go to interrupt service rti; ( routines ProcessRDCInit: {=================== BEGIN INIT LOOP CONTROL CODE ICNTL=hit000C; ( Set interrupt control register ( for non-nested and edge- { triggered level 2 4 3 ( interrupts IMASK=h#0000; ( Set interrupt mask register { to mask off all interrupts AXO = h#4000; Get default output value ) dm(θutputPort)=AXO; ( Perform hardware reset dm(ControlWord) =AX0; { Save the output value INITIALIZE TIMERS INITIALIZE TIMERS AXO = h#0036; Set up timer control word dm(DSPTimer)=AXO for square wave mode AXO = httOODO; Load initial count for 4 KHz d (DSPIRQ) =AX0 hdwr servo interrupt AXO = h#0007; dm(DSPIRQ) =AX0 AXO = h#0076; Set up timer control word dm(DSPTimer)=AXO for square wave mode AXO = h#00C4; Load initial count for 3200 Hz dm(SpinRef) =AX0 gyro spin motor ref AXO = h#0009; dm(SpinRef) =AX0 AXO = h#00B6; Set up timer control word dm(DSPTimer)=AXO for square wave mode AXO = h#00C8; Load initial count for 40 KHz dm(GyroRef) =AX0 gyro pickoff ref AXO = h#0000; dm(GyroRef) =AX0 SUSSTTTUTE SHEET RULE 26 AXO = 10; dm(SampCount) = AXO; AXO = 0; dm(RefSampleDone) = AXO; Init to SampleRef dm(YRefReal) = AXO; dm(YReflmag) = AXO; dm(PhaseClockToRef) = AXO ;( dm(PhaseRotorToStator) = AXO; { dm(CosTotalPhase) = AXO; dmisinTotalPhase) = AXO; d (RefCmdToggle) = AXO; 10 = Λ YRef; CNTR= %YRef; Set counter with buffer length do ClearYRef until CE; Clear YRef buffer ClearYRef: dm(10,Ml) = AXO; 10 = Λ CosRef; CNTR= %CosRef; Set counter with buffer length do ClearCosRef until 'ZZ ; Clear CosRef buffer ClearCosRef: dm(10,Ml) = AXO; dm(YCo≤Real) = AXO dm(YCosIrnag) = AXO dm(YSinReal) = AXO dm(YSinlmaq) = AXO dm(PyyCos) = AXO; dm(PyySin) = AXO; dm(QQ) = AXO; dm(CosSum) = AXO; dm(SinSum) = AXO; 10 = Λ YCos; CNTR= %YCos; Set counter with buffer length do ClearYCos until CE; Clear YCos buffer ClearYCos: dm(10,Ml) = AXO; 10 = Λ YSin; CNTR= %YSin; Set counter with buffer length do ClearYSin until CE; Clear YSin buffer ClearYSin: dm(10.Ml) = AXO; 10 = Λ YRef; ( Init 10 for Reference sampling) AXO = 8000; dm(RefSettleDelayCount) = AXO; ( Determine PhaseRotorToStator using oscilloscope. To be determined by software at a later date. ) WRAP UP AND WAIT FOR INTERRUPTS ena AR_SAT; { Enable ALU saturation mode } IMASK=h#000C; ( Enable edge-triggered inter- ) { rupts IRQ2 and IRQ3 } WaitForlnterrupt: nop; { ) jump WaitForlnterrupt; ( Wait for 4kHz interrupt ) END INITIALIZATION .newpage; { • INTERRUPT SERVICE ROUTINE FOR INTERRUPT 2 ProcessCntllRQ: nop; AXO = httDFFF ; AYO = d (ControlWord) ; else DSP:=PeriCntl AR = AXO and AYO ; Startlbit:=true dm(OutputPort) =AR; dm(ControlWord)=AR; ( Set up main loop so that there are two paths, either/or. First is LE 26 reference sampling, which continues for first ten samples at 4 kHz, after which a flag is set to switch the routine to normal operation for the remainder of the interrupts. Normal operation consists of sampling the two RDC inputs,' sin and cos, adding to accumulation, and calculating position every ten interrupts. Gimbal DAC value is also output on every tenth interrupt. AYO = dm(AzDriftCmd) ; • AYO = h#147B; ! = xx.xx deg; h#147B = 2S.8 deg } dm(PhaseRotorToStator) = AYO; Generate 400 Hz reference signal ) AXO = dm(SampCount) ; AYO = 10; AR = AXO - AYO; if EQ jump SetSquareOut; AYO = 5; AR = AXO - AYO; if EQ jump ClearSquareOut; jump CheckDelay; SetSquareOut: AXO = h#FFFF; dm(AzPosnDC) = AXO; jump CheckDelay; ClearSquareOut: AXO = h#8000; dm(AzPosnDC) = AXO; CheckDelay: AYO = dm(RefSettleDelayCount) AR = pass AYO; if EQ jump WhichPath; AR = AYO - 1; dm (RefSett leDelayCount ) = AR ; WhichPath: AXO = dm (RefSaιtιρleDone ) ; ( AR = pass AXO ; if NE jump DoRDCProc; call SampleRef; jump FinishlSR; DoRDCProc: call SimulRDC; ( AcquirePosn: dm(StartRDC) =AR; ( */ Inhibit Az t El RDCs /* ) CNTR= 24; do WaitForRDCl until CE; WaitForRDCl: nop; (Wait 3 microseconds ) AYO = h#8000; (Load mask to invert MSB ) AR = dπι(AzPosnFdbk) tRead RDC channel 1 } AR = AR xor AYO; {Invert MSB to get 2s complement) dmIAzPosnOut) =AR; dm(RDCAzPosn) =AR; nop; AYO = h#8000; AR = dm(ElPosnFdbk) (Read RDC channel 2 } AR = AR xor AYO; (Invert MSB to get 2s complement) dm(ElPosnθut) =AR; nop; dm(S opRDC)=AR; FinishlSR: AR = dm(QQ); dm(AzAdjPosnOut) AR; AY1 = httβOOO; AR = AR xor AY1; ( TEST CODE - output qq signed ) ReleaseCPUInhibit: AXO = h#2000; AYO = dm(ControlWord) else DSP:=PeriCntl AR = AXO or AYO; Startlbit:=true dm(θutputPort) =AR; dm(ControlWord) =AR; ServoDone: rti; .ENDMOD; { END OF MAIN LOOP .MODULE/ROM PDC_Reference_Phase; ( ADSP 2100 RDC Reference signal characterization Author: John Anagnost/ M. Kieffer 8/03/94 Input : None Output : CosRef array values, state of RefSampleDone logical. Altered Registers: 10, II, 13, 14, 15, AXO, AX1, AYO, AY1, AR, AF, MXO, MX1, MYO, MY1, MR, MF, SRO, SRI, SI Description: Determine phase of reference signal relative to system clock. The notation is as follows: yr(l)= first sampled reference value at t=t0, yr(2) is the second sampled reference value at t=t0+l/4000, ...yr(10) is the tenth sampled reference value at t=t0 + 10/4000. yr = ADC sampled reference value - use another interrupt to trigger sampling, disable this interrupt after init has completed. 09/30/94: Added code to prevent exiting reference sample mode until reference has settled out. MHK ) .external OutputPort; .external StartADC; .external AnalogSample; .external YRefReal; .external YReflmag; .external PhaseClockToRef; .external PhaseRotorToStator; .external CosTotalPhase; .external SinTotalPhase; .external YRef; .external CosRef; .external SampCount; .external RefSampleDone; .external ControlWord; .external YCos; .external YSin; .external CosLookup; .external SinLookup; .external arctan; .external sin; .external RefSettleDelayCount; .ENTRY SampleRef; SampleRef: AYO = h#E0FF; ( ADC Mux mask clear ) AR = d (ControlWord) AR = AR and AYO; AYO = h#0C00; ( RDC Ref input mask - U5,pin 12 } AR = AR or AYO; dm(ControlWord)=AR; dm(OutputPort) =AR; CNTR= 15; ( ADC Mux settling time ) do WaitForMUXl until CE; WaitForMUXl: nop; dm(StartADC)=AY0; CNTR= 91; { ADC conversion time ) do WaitForADCl until CE; WaitForADCl: nop; AR = dm(AnalogSample) AYO = h#FFF0; AR = AR and AYO; dm(I0,Ml)=AR; ( Store YRef AYO = dm(SampCoun ) ; AR = AYO - 1; if EQ jump ComputeRef Phase; dm(SampCount) = AR; jump ExitSampRef; ComputeRefPhase: AXO = 10; dm(SampCount) = AXO; 10 = Λ YRef; AXO = d (RefSettleDelayCount) AR = pass AXO; if GT jump ExitSampRef; L0 = 0 LI = 0 L4 = 0 L5 = 0 Ml = 1 M5 = 1 ( YRefReal = YRef(1) 'CosLookup(1) YRef(2) 'CosLookup(2) YRef(3) 'CosLookup(3) YRef(4)'CosLookup(4) YRef(5) 'CosLookup(5) YRef(6)*CosLookup(6) YRef(7) 'CosLookup(7) YRef(8) 'CosLookup(8) YRef(9) 'CosLookup(9) YRef(10) 'CosLookup(10) 10 = Λ YRef; 14 = "CosLookup; MR = 0; MX0 = d (10,Ml); MY0 = pm(I4,MS) ; CNTR = 10; do RefSampSumReal until CE; RefSampSumReal: MR = MR+MX0'MY0 (ss) , MX0 dm(IO.Ml), MYO = pm(I4,M5); if MV sat MR; MX1 = MR1; MY1 = MR0; MR = MR (rnd) ; if MV sat MR; dm(YRefReal) = MR1; ( YReflmag = YRef(1) 'SinLookup(l) + YRef(2) 'SinLookup(2) ♦ YRef(3)*SinLookup(3) + YRef(4)'SinLooku (4) + YRef(5) 'SinLookup(5) + YRef(C)*SinLookup(6) + YRef(7) 'SinLookup(7) + YRef(8) 'SinLookup(8) ♦ YRef(9)*SinLookup(9) ♦ YRe (10)'SinLooku (10) ; 10 YRef; 14 "SinLookup; MR = 0; MX0 = d (10,Ml) ; MYO = pm(I4,M5) ; CNTR = 10; do RefSampSumlmag until CE; RefSampSumlmag: MR = MR+MX0*MY0 (ss) , MX0 = dmdO.Ml), MYO = pm(I4,M5); if MV sat MR; SRI = MR1; SR0 = MRO; MR = MR (rnd) ; if MV sat MR; d (YReflmag) = MR1; ( Determine phase angle using arctan algorithm. if abs(YReflmag) < abs(YRefReal) , PhaseClockToRef=atan(YReflmag/YRefReal) else PhaseClockToRef=0.5-atan(YRefReal/YReflmag) ) AXO = d (YRefReal); AR = abs AXO; AX1 = AR; ( abs(YRefReal) ) AXO = dm(YReflma >; AR = abs AXO; AYl = AR; ( abs (YReflmag) ) AR = AYl - AXl; if EQ jump RealBigger; if LT jump RealBigger; ( PhaseClockToRef=0.5-atan(YRefReal/YReflmag) ImagBigger: AYl = MX1; { MSW of YRefReal ) AYO s MY1; ( LSW of YRefReal ) AXl = dm(YReflmag); DIVS AYl, AXl; DIVQ AXl; DIVQ AXl; DIVQ AXl DIVQ AXl; DIVQ AXl; DIVQ AXl DIVQ AXl; DIVQ AXl; DIVQ AXl DIVQ AXl; DIVQ AXl; DIVQ AXl DIVQ AXl; DIVQ AXl; DIVQ AXl AR = AYO; MYO = 1; MR = AR * MYO (SS) ; CallArctanlmagBig: call arctan; AR = -AR; AYO = h#4000; AR = AR + AYO; d (PhaseClockToRef) = AR; {0.5-arctan(YRefReal/YRefImag) ) jump CalcTotalPhase; ( PhaseClockToRef=atan(YRefImag/YRefReal) } RealBigger: AYl = SRI; ( MSW of YReflmag ) AYO = SRO; { LSW of YReflmag ) AXl = dm(YRefReal); DIVS AYl, AXl; DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl DIVQ AXl AR = AYO MYO = 1; MR = AR MYO (ss) CallArctanRealBig: call arctan; d (PhaseClockToRef) = AR; ( arctan(YRefImag/YRefReal) ) { SinTotalPhase = sin(PhaseRotorToStator-PhaseClockToRef) ; CosTotalPhase = sin(0.5 * PhaseRotorToStator+PhaseClockToRef) ; ) CalcTotalPhase: nop; AYO = d (PhaseRotorToStator) ; AXO = AR; dis AR_SAT; AR = AYO - AXO; ena AR_SAT; AXO = AR; ( PhaseRotorToStator - PhaseClockToRef } call sin; d (SinTotalPhase) = AR; AYl = h#4000 ; dis AR_SAT; AR = AXO + AYl ; ( 0 .5 + PhaseRotorToStator- PCTR ) ena AR_SAT; AXO = AR; call sin; AssignCosTotalPhase: dm(CosTotalPhase) = AR; CosRef(1) CosLookup(1) 'CosTota 1Phase SinLookup(l) •SinTotalPhase; CosRef(2) CosLookup(2) 'CosTota1Phase SinLookup(2) • SinTotalPhase; CosRef(3) CosLookup(3) 'CosTota1Phase SinLookup(3) 'SinTotalPhase; CosRef(4) CosLookup(4) 'CosTota1Phase SinLookup(4) •SinTotalPhase; CosRef (5) CosLookup(5) • CosTota1Phase SinLookup(5) •SinTotalPhase; CosRef(6) CosLookup(6) • CosTota1Phase SinLookup(6) • SinTotalPhase; CosRef(7) CosLookup(7) • CosTota1Phase SinLookup(7) •SinTotalPhase; CosRef(8) CosLookup(8) • CosTota1Phase SinLooku (8) •SinTotalPhase; CosRe (9) CosLookup(9) • CosTota1Phase SinLookup(9) •SinTotalPhase; SUBSTTTUTESHEET(RUIE26) CosRef (10) = CosLookup (10) 'CosTotalPhase - SinLookupt 10 ) 'SinTotalPhase; } 10 = CosRef: 14 = CosLookup,* 15 = "SinLookup; MXO = pm(I4,M5) ; MYO = dm(CosTotalPhase) ; AR = dm(SinTotalPhase) ; AR = -AR; MY1 = AR; CNTR = 10; do CosRefCalc until CE; MR = MXO'MYO (ss), MXO = pm(I5,M5); MR = MR+MX0*MY1 (rnd), MXO = pm(I4,M5); if MV sat MR; CosRefCalc: dm(10,Ml) = MR1; 10 = "YCos; ( Assign 10 to YCos array ) 11 = "YSin; ( Assign 10 to YSin array j AXO = 1; d (RefSampleDonei AXO; ExitSampRef: rts; .ENDMOD; lf 26 .MODULE/ROM RDC_Simulatιon; ( ADSP 2100 RDC simulation algorithm Author: John Anagnost/ M. Kieffer 8/03/94 Input : None Output : CosRef array values, state of SampleRefDone logical. Altered Registers: 10, II, 12, 13, 14, 15, AXO, AXl, AYO, AYl, AR, AF, MXO, MX1, MYO, MY1, MR, MF, SRO, SRI, SI Compute every 1/400 seconds. Data should be taken at 4 KHz intervals. The notation is as follows: yc(l) = first sampled cosine value at t=t0, yc(2) is the second sampled cosine value at t=t0+l/4000, ...yc(lθ) is the tenth sampled cosine value at t=t0 + 10/4000. The notation is similar for the sampling of the sine winding, yε(l), ys(2), ... ys(lC) F; { ADC Mux mask clear ) AR = dm(ControlWord) ; AR = AR and AYO; AYO = h#1600; ( RDC Cos input mask - A2-P2-A31 ) AR = AR or AYO; dm(ControlWord)=AR; dm(Outpu Port) =AR; CNTR= 15; ( ADC Mux settling time ) do WaitForMUX2 until CE; WaitForMUX2: nop; dm(StartADC) =AY0; AYO = h#1700; ( RDC Sin input mask - A2-P2-A32 ) AR = AR or AYO; dm(ControlWord)=AR; d (Outpu Port) =AR; CNTR= 86; ( ADC conversion time } do WaitForADC2 until CE; WaitForADC2 : nop; AR = dm(AnalogSaπιple) ; AYO = h#FFF0; dm(StartADC)=AYO; AR = AR and AYO; dm(I0,Ml)=AR; ( Store YCos(k) AYl = h#S000; ( TEST CODE } AR = AR xor AYl; ( TEST CODE } dm(ElPosnDC) = AR; ( TEST CODE ) CNTR= 91; ( ADC conversion time ) do WaitForADC3 until CE WaitrorADC3: nop; AR = dm(AnalogSample) ; AR = AR and AYO; dm(Il,Ml)=AR; ( Store YSin(k) AR = AR xor AYl; { TEST CODE ) dm(ElPosnDC) = AR; ( TEST CODE ) AYO = d (SampCount) ; AR = AYO - 1; if EQ jump ComputeRDCAngle; dm(SampCount) = AR; jump Exi DCSimul; ComputeRDCAngle: AXO = 10; dm(SampCount) = AXO; ( YCosReal = YCos(l) •CosLookup(l) YCos(2) 'CosLooku (2) YCos(3) • CosLookup(3) YCos( ) 'CosLookup(4) YCos(5) *CosLookup(5) YCos(6)'CosLookup(6) YCos(7) •CosLookup(7) YCos(8) 'CosLookup(8) YCos(9) • CosLookup(9) YCos(10)'CosLookup(10) 10 = "YCos; 14 = "CosLookup; MR = 0; CNTR = %YCos; MXO = d (10,Ml); MYO = pm(I4,M5) ; do YCosRealSum until CE; YCosRealSum: MR = MR+MXO'MYO (ss), MXO d (10, l), MYO = pm(I4,M5) MR = MR (rnd) ; if MV sat MR; dm(YCosReal) = MR1; ( YCosImag YCosd)'SinLookup(l) YCos(2)*SinLookup(2) YCos(3) *SinLookup(3) YCos(4)'SinLookup(4) YCos(5) 'SinLookup(S) YCos( C) 'SinLookup(6) YCos(7) •SinLookup(7) YCos(8) 'SinLookup(8) YCos(9) *SιnLookup(9) YCos(10) 'SinLookup(10) 10 = "YCos; 15 = "SinLookup; MR = 0; CNTR = %YCos; MXO = dm(10,Ml) ; MYO = pm(I5,M5) ; do YCosImagSum until CE; YCosImagSum: MR = MR+MX0'MY0 (ss), MXO = dm(I0,Ml). MYO = pm(I5,M5) MR = MR (rnd) ; if MV sat MR; dm(YCosImag) = MR1; ( YSinReal = YSin(1)'CosLookup(1) YSin(2)'CosLookup(2) YSin(3)'CosLookup(3) YSin(4)*CoεLookup(4) YSin(5) 'CosLookup(5) YSin(6) 'CosLookup(6) YSin(7) » CosLookup(7) YSin(8) 'CosLookup(8) YSin(9) 'CosLookup(9) YSin(10) 'CosLookup(10) DoYSinReal: II = "YSin; 14 = "CosLookup; MR = 0; CNTR = %YSin; MXO = dm(II,Ml) ; MYO = pm(I4,M5) ; do YSinRealSum until CE; YSinRealSum: MR = MR+MXO'MYO (ss) , MXO d (II,Ml) , MYO = pm(I4,M5) MR = MR (rnd) ; if MV sat MR; dm(YSinReal) = MR1; ( YSinlmag = YSin(1) 'SinLookup(1) YSin(2)'SinLookup(2) YSin(3) 'SinLookup(3) YSin(4) 'SinLookup(4) YSin(5)'SinLookup(5) YSin(6)SinLookup(6) YSin(7) 'SinLookup(7) YSin(8) 'SinLookup(8) YSin(9) 'SinLookup(9) YSin(10) 'SinLookup(10) II = "YSin; 15 = "SinLookup; MR = 0; CNTR = %YSin; MXO = dm(II,Ml) ; MYO = ptn(I5,M5) ; do YSinlmagSuiti until CE; YSinlmagSum: MR = MR+MX0*MY0 (εs), MXO = d (II,Ml), MYO pm(I5,M5) MR = MR (rnd) ; if MV sat MR; dm(YSinlmag) = MR1; ( PyyCos = YCosReal"2+YCosImag"2; ) { PyySin = YSinReal"2+YSinImag"2; ) MXO = dm(YSinReal) ; MYO = MXO; MR = MXO • MYO (ss) ; MXO = din(YSinlmag) ,* MYO = MXO; MR = MR ♦ MXO * MYO (sε) ; if MV sat MR; SRI = MR1; ( PyySin HI 1 SRO = MRO; ( PyySin LO ) MR = MR (rnd) ; if MV sat MR; dm(PyySin) = MR1; TestLabel: nop; { Compute angle using numerical square root algorithm and Analog Devices' arc tangent algorithm. See accompanying sheets for algorithms. Angle is scaled by -1 = -180 and 1 = 180 if PyySin < PyyCos, qq = atan(sqrt5 (PyySin/ yyCos) ) ; else qq = 0.5 - atanfsqrtδ (PyyCos/PyySin) ) ; ) AXl = d (PyyCos) ; AYl = d (PyySin); AR = AYl - AXl; if EQ jump PyyCosBigger; if LT jump PyyCosBigger; ( QQ = 0.5 - atanlsqrtS(PyyCos/PyySin) ) PyySinBigger: AYl = MX1; ( MSW of PyyCos ) AYO = MY1; ( LSW of PyyCos ) AXl = dm(PyySin) ; DIVS AYl AXl; DIVQ AXl; DIVQ AXl DIVQ AXl DIVQ AXl; DIVQ AXl DIVQ AXl DIVQ AXl; DIVQ AXl DIVQ AXl DIVQ AXl; DIVQ AXl DIVQ AXl DIVQ AXl; DIVQ AXl DIVQ AXl SRO = AYO call sqrtS; ( sqrt5(PyyCos/PyySin) ) MYO = 1; MR = SRI • MYO (ss) ; call arctan; AYO = h#4000; AR = -AR; AR = AR + AYO dm(QQ) = AR; { 0.5 - arctan(sqrtδ (PyyCos/PyySin) ) } jump FindSign; PyyCosBigger: AYl = SRI; ( MSW of PyySin ) AYO = SRO; ( LSW of PyySin I AXl = dm(PyyCos) ; DIVS AYl, AXl; AXl; DIVQ AXl AXl; DIVQ AXl AXl; DIVQ AXl AXl; DIVQ AXl AXl; DIVQ AXl sqrt5(PyySin/PyyCos) } TestLabell: (ss TestLabel2: TestLabel3: ( Determine sign of resolver angle ) FindSign: nop; dm(PreSignQQ) = AR; 10 = "YCos; 12 = "CosRef; MR = 0; ( CosSum YCosd) 'CosRef(1) YCos(2) 'CosRef(2) YCos(3)'CosRef(3) YCos(4)'CosRe (4) YCos(5) 'CosRef (5) YCos(6) 'CosRef(6) YCos(7) 'CosRef (7) YCos(8) 'CosRef(8) YCos(9) 'CosRef(9) YCos(10) 'CosRef(10) CNTR = %YCos; MXO = d (10,Ml) ; MYO = dm(I2,Ml); do CalcCosSum until CE; MR = MR+MX0*MY0 (ss), MXO = dm(I0,Ml); CalcCosSum: MYO = d (12,Ml); MR - MR (rnd) ; if MV sat MR; dm(CosSum) = MR1; 11 = "YSin; 12 = "CosRef; MR = 0; SinSum = YSind) 'CosRef(1) YSind) 'CosRef(2) + YSin(3) 'CosRef(3) YSin(4)•CosRef(4) ♦ YSin(5)'CosRef(5) YSin(6)*CosRef(6) + YSin(7) 'CosRef(7) + YSin(8) 'CosRef(8) + YSin(9) 'CosRef(9) + YSin(lO) 'CosRef(10) ; ) CNTR = %YSin; MXO = d (II,Ml) ; MYO = dm(12,Ml) ; do CalcSinSum until CE; MR = MR+MXO'MYO (ss) , MXO = dm(II,Ml) ; CalcSinSum: MYO = d (12,Ml) ; MR = MR (rnd) ; if MV sat MR; d (SinSum) = MR1; ( if SinSum => 0, if CosSum => 0, qq = qq; else qq = 1 - qq; elseif CosSum => 0, qq = -qq; else qq = qq - 1; } AR pass MR1; if EQ jump SinSumGEO; if GT jump SinSumGEO; SinSumLTO: AXO = dm(CosSum) ; AR = pass AXO; if EQ jump NegateQQ; if GT jump NegateQQ; AYO = httβOOO; AR dm(QQ) ; AR AR + AYO; ( qq = qq - 1 ) jump StoreQQ; NegateQQ: AR = dm(QQ) ; AR = -AR; jump StoreQQ; SinSumGEO: AXO = d (CosSum) ; AR = pass AXO; if EQ jump StetQQ; if GT jump StetQQ; AR = dm(QQ) ; AYO = h*8000; ( -1 I AR = AR - AYO ( qq - 1 1 AR = -AR; ( 1 - qq ) StoreQQ: dm(QQ) = AR; StetQQ: AYO = 0; ( d (ElDriftCmd) ; AR = AYO - 1; if NE jump ToggleRefCmd; AXO = d (Re CmdToggle) ; AR = pass AXO; if EQ Du p Rese RefSampDone jump ResetSinCos; ToggleRefCmd: AXO = 0; dm(RefCmdToggle) = AXO; ResetSinCos: 10 = "YCos; 11 = "YSin; AXO = dm(QQ) ; jump E it DCSimul; ResetRefSampDone: AXO = 0; dm(RefSampleDone) = AXO; AXO = 1; dm(RefCmdTogg1e) = AXO; 10 = "YRef; Exit DCSimul: nop; AR dm(QQ); AYl h#8000; { TEST CODE - output qq ) AR AR xor AYl; ( TEST CODE - output qq } dm(ElPosnDC) = AR; ( TEST CODE - output qq ) rts .ENDMOD; { Use qq as angle for position loops, with the before mentioned scaling: -1 = -180 , 1 = 180 . ) π TE SHEET ROUE 26 . MODULE/ROM Sin_Approximation ; ( Sine Approximat ion Y = Sin ( x) Calling Parameters AXO = x in scaled 1 . 15 format Ml = 1 L3 = 0 Return Values AR = y in 1 .15 format Altered Registers AY0,AF,AR,MY1,MX1,MF,MR,SR,I3 Computation Time 25 cycles) .VAR/DM sin_coeff[5] ; •INIT sin_coeff : HK3240, HS0053, HItAACC, H#08B7, HItlCCE; .ENTRY sin; sin: I3="sin_coeff,- (Pointer to coeff. buffer} AY0=H#4000; AR=AX0, AF=AX0 AND AYO; (Check 2nd or 4th quad.) IF NE AR=-AX0; (If yes, negate input) AY0=H#7FFF; AR=AR AND AYO; (Remove sign bit) MY1=AR; MF=AR'MY1 (RND), MX1=DM(I ,Ml) ; (MF = x* « 2) MR=MX1 « MY1 (SS), MX1=DM(I3,Ml) ; (MR = Cl » x) CNTR=3; DO ApproxSine UNTIL CE; MR=MR+MX1'MF (SS) ; ApproxSine: MF=AR*MF (RND), MX1=DM(I ,Ml) , MR=MR+MX1*MF (SS) ; SR=ASHIFT MR1 BY 3 (HI) ; SR=SR OR LSHIFT MRO BY 3 (LO) (Convert to 1.15 format) AR=PASS SRI; IF LT AR=PASS AYO; (Saturate if needed) AF=PASS AXO; IF LT AR=-AR; (Negate output if needed) RTS; .ENDMOD; . MODULE/ ROM Square_Root ; ( ADSP 2100 RDC square root algorithm Author : John Anagnost/ M . Kieffer 8 / 02 /94 Input is assumed to be in the interval [0 1] . Registers used: AXO, AXl, AYO, AYl, SR, MR, MXO, MXl, MYO, MYl, AR, AF Input : SRO Output: SRI Result is in 1.15 format if input is.) .external SqrtlterCount; { Counts iterations of algorithm } .ENTRY sqr δ; Sqrt5: AXO = SRO; Xl ) AR = pass AXO; if NE jump InitX2; MXO = 0; jump SqrtDone; InitX2: AYO = h#7FFF; ( x2 ) MXO = SRO; ( x3 ) MYO = h#4000; AXl = 0; dm(SqrtlterCount) = AXl; AYl = 0; AF = AXl + AYl; SqrtLoop: AYl = dm(SqrtlterCount) AR = AYl •> 1; dm(SqrtlterCount) = AR; AR = AXO - AYO; MR = AR • MYO (rnd); AR = abs MR1; AYl = 2; AR = AR - AYl; if LT jump SqrtDone; ( if abs(xl-x2)/2> 2 LSBs, ) MXl = AXO; ( Xl ) MR = MXl * MYO (ss); ( (xl)/2 I MXl = AYO; ( x2 ) MR = MR ♦ MXl ' MYO (rnd); I (xl L++xx22))//22 ) MXO = MR1; ( x3=(xl+x2) /2; ) { if sign(x3"2-x)osign(xl"2-x) , I AYl = SRO; ( Assign AYl = x MYl = MXO; ( AXl = sigπ(MX0"2-AYl) MR = MXO MYl (rnd),*( (x3)"2 ) AR = MR1 AYl; ( (x3)"2 - x I AR = 0; if LT AR = AF + 1 ( if (x3)"2 - x neg, AXl = 1 ) AXl = AR; { else AXl = 0 ) MXl = AXO; ( AYl = sign(AX0"2-AYl) ) MYl = AXO; MR = MXl MYl (rnd);( (xl "2 .MODULE/ROM Arctan_Approximation,* ( Arctangent Approximation y = Arctan( ) Calling Parameters MR1 = Integer part of x MRO = Fractional part of x Ml = 1 L3 = 0 Return Values AR = Arctan(x) in 1.15 scaled format (-0.5 for -90 degrees, 0.5 for 90 degrees) Altered Registers AXO,AXl,AYO,AYl,AR,A ,MYO, Yl,MXl,MF,MR,SR,SI Computation Time 58 cycles (maximum) VAR/DM/RAM atn_coeff[5] ; INIT atn_Cθeff : H#28BD, Hft006D, H#EF3E, H#0SC6, HKFED4; .ENTRY arctan; • arctan 13 = "atn_coeff; (Point to coefficients) AY0=0; AX1=MR1; AR PASS MR1; IF GE JUMP posi; (Check for positive input) AR=-MR0; (Make negative number positive) MR0=AR; AR=AY0-MR1+C-1; MR1=AR; posi: SR=LSHIFT MRO BY -1 (LO) Produce 1.15 value in SRO) AR=SR0; AY1=MR1; AF=PASS MR1; IF EQ JUMP noinv; (If input < 1, no need to invert) SE=EXP MR1 (HI) ; (Invert input) SR=NORM MR1 (HI) ; SR=SR OR NORM MRO (LO) ; AX0=SR1; SI=H#0001; SR=NORM SI (HI) ; AY1=SR1; AY0=SR0; DIVS AY1,AXO; DIVQ AXO; DIVQ AXO; DIVQ AXO DIVQ AXO; DIVQ AXO; DIVQ AXO DIVQ AXO; DIVQ AXO; DIVQ AXO DIVQ AXO; DIVQ AXO; DIVQ AXO DIVQ AXO; DIVQ AXO; DIVQ AXO AR=AY0; noinv: MY0=AR; MF=AR*MY0 (RND) MY1=DM(I3,M1) ; MR=AR'MY1 (SS), MX1=DM(I3,M1); CNTR=3; DO ApproxArctan UNTIL CE; MR=MR+MX1*MF (SS) MX1=DM(I3,M1) ; ApproxArctan: MF=AR*MF (RND) ; MR=MR+MX1'MF (SS) ; AR=MR1; AY0=H#4000; AF=PASS AYl; IF NE AR=AY0-MR1; AFxPASS AXl; IF LT AR=-AR; RTS* •ENDMOD; SUBSTTII SHEET RULE 26

Claims: CLAIMS What is Claimed is: 1. A gimbal system, comprising: instrumentation mounted on a gimbal; control circuitry for controlling said instrumentation; a resolver for measuring parameters of an angle of rotation of said instrumentation as said instrumentation rotates on said gimbal, said resolver outputting said measured parameters as analog output parameter signals, along with an analog reference signal; multiplexor means for multiplexing said parameter and reference signals; an analog to digital converter, coupled to the multiplexor means, for converting said parameter and reference signals to digital signals; digital processing means for performing system computations based on data collected from said instrumentation, said digital processing means including: a) filter means for receiving said signals from said multiplexor means and for filtering said signals, thereby reducing unwanted noise and signal sensitivity; b) computing means for digitally computing a value for said angle of rotation of the instrumentation from the signals from said lter means; and c) means for coupling said computed angle of rotation value to said control circuitry; wherein said system minimizes hardware component requirements. 2. The system of Claim 1, wherein said computing means is programmed to perform fast Fourier transform calculations using said parameter and reference signals to compute said angle of rotation value. 3. The system of Claim 1, wherein said computing means comprises: means for computing a magnitude value of said angle of rotation from said parameter and reference signals; means for computing quadrant and resulting sign values of said angle of rotation from said parameter and reference signals; and means for computing quadrature voltage values for said parameter and reference signals. 4. The system of Claim 1, wherein said parameter and reference signals from said resolver comprise differential signals. 5. The system of Claim 4, further comprising means for converting said differential signals from said resolver to single-ended signals before said signals are input into said multiplexor means. 6. The system of Claim 1, further comprising digital post-filtering means for post-filtering said computed value of said angle of rotation, before said computed value is coupled to the control circuitry. 7. The system of Claim 6, wherein said post-filtering means comprises an averaging block. 8. The system of Claim 6, wherein said post-filtering means comprises a digital low pass filter. 9. The system of Claim 1, wherein said filter means comprises means for implementing a Hanning window. 10. The system of Claim 1, wherein said filter means comprises means for implementing a Blackman window. 11. The system of Claim 1, wherein said filter means comprises means for implementing a rectangular windowing function. 12. The system of Qaim 1, further comprising an amplifier for scaling said parameter and reference signals. 13. A method for determining the angle of rotation of gimbal mounted instrumentation in a gimbal system having control circuitry for controlling said instrumentation, comprising the steps of: a) receiving signals from said instrumentation; b) measuring parameters of said angle of rotation from said received signals; c) outputting said parameters as analog signals along with an analog reference signal; d) converting said signals from differential signals to single ended signals; e) multiplexing said signals; f) converting said analog signals to digital signals; g) filtering said multiplexed, single ended digital signals to reduce noise and signal sensitivity; h) digitally computing an angle of rotation value from said digital signals; and i) outputting said computed value for said angle of rotation to said control circuitry controlling said instrumentation. 14. The method of Qaim 13, wherein said step b) comprises determining sine and cosine values for said angle of rotation. 15. The method of Claim 13, wherein said step h) value comprises: computing a magnitude value for said angle of rotation; computing quadrant and resulting sign values of said angle of rotation; computing quadrature voltage values associated with said angle of rotation parameters; and digitally post-filtering signals representing said angle of rotation before outputting said signals to said control circuitry to reduce noise associated with said signal. SUBSTITUTE SHEET (RUl£ 26) 16. A gimbal system, comprising: instrumentation mounted on a gimbal; control circuitry for controlling said instrumentation; a resolver for measuring parameters of an angle of rotation (θ) of said instrumentation as said instrumentation rotates on said gimbal, said resolver outputting said measured parameters as analog output parameter signals, along with an analog reference signal; multiplexor means for multiplexing said parameter and reference signals; an analog digital convertor coupled to the multiplexor means for converting said parameter and reference signals to digital signals; digital processing means for performing system computations based on data collected from said instrumentation, said digital processing means including: a) filter means for receiving said signals from the multiplexor means and for filtering said signals, thereby reducing unwanted noise and signal sensitivity; b) computing means for digitally computing a value for the magnitude of said angle of rotation of the instrumentation through the following equation: where Y llB is a sine output from said resolver, f ref is a reference frequency, q s is a quadrature magnitude for a sine winding of said resolver, and q e is a quadrature magnitude for a cosine winding of said resolver; c) means for computing a quadrant of said angle of rotation by computing values from the following equations: Acos (9) (2) Asin (β) (3) where A is a constant and θ is said angle of rotation; said angle of rotation being located in a first quadrant if computed values for equations (2) and (3) are positive, said angle of rotation being located in a second quadrant if said computed value of equation (2) is negative and said computed value of equation (3) is positive, said angle of rotation being located in a third quadrant if said computed values of equations (2) and (3) are negative, said angle of rotation being located in a fourth quadrant if said computed value of equation (2) is positive and said computed value of equation (3) is negative; d) means for estimating quadrature voltage values for said sine and cosine windings of said resolver by detecting a voltage associated with said instrumentation as said instrumentation rotates through a zero degree reference point of said resolver, and then subtracting said estimated quadrature voltage; and e) means for outputting said system computations to said control circuitry to control said instrumentation. SUBSTTTUTE SHEET (RULE 26)

Inventors: ANAGNOST JOHN J, ANAGNOST, JOHN, J.
Applicants: HUGHES AIRCRAFT CO, HE HOLDINGS, INC., DOING BUSINESS AS, HUGHES ELECT
Application year: 1996
Main classification: G01S3
IPC codes: G01S3, H03M1