Digital circuitry and method for calculating inclinometer angles

Abstract

A method is disclosed for performing calculations for an inclinometer device, as is a digital circuitry for performing such calculations. The circuitry comprises an interface for receiving detection signals from a sensor device and a CORDIC unit for performing calculation of inclinometer output values characterizing a resultant vector. The CORDIC calculation unit is configured to perform a calculation for resolving the angle between a resultant vector and a programmable reference value using hyperbolic CORDIC calculation. Pre-rotation may be performed for a vector before hyperbolic CORDIC arctangent calculation phases.

Claims

1. A digital circuitry configured to perform calculations for an inclinometer device, the circuitry comprising: an interface configured to receive detection signals from a sensor device; and a CORDIC unit configured to perform calculations of inclinometer output values characterizing a resultant vector, wherein the CORDIC unit is configured to perform a first calculation for resolving an angle between a resultant vector representing the detection signals and a programmable reference value, the first calculation including at least two hyperbolic CORDIC calculation phases, wherein the first calculation comprises: determining a first intermediate result corresponding to a length of a projection of the resultant vector on a reference axis times a multiplier representing the change of the length of the vector by using a hyperbolic CORDIC arctangent function; determining a second intermediate result corresponding to a length of a component of the resultant vector orthogonal with a reference axis multiplied with a multiplier representing a change of the length of the vector using the hyperbolic CORDIC arctangent function; and determining an angle between the resultant vector and a calculated axis orthogonal to the reference axis based on the first and the second intermediate results using a circular CORDIC arctangent function.

2. The digital circuitry according to claim 1, wherein the programmable reference value corresponds to an effect of normal gravity along a defined reference axis.

3. The digital circuitry according to claim 1, wherein the result of the first calculation equals to an angle calculated with an arcsine function between the resultant vector and the reference value, and wherein the circuitry is configured to perform at least one pre-rotation prior to performing any one of the hyperbolic CORDIC arctangent calculations in order to expand a convergence condition of the resulting arcsine function.

4. The digital circuitry according to claim 1, wherein the CORDIC calculation unit is further configured to perform at least one of: a second calculation for resolving an angle between a resultant vector or a projection of the resultant vector and an axis in a plane defined by two axes, the second calculation including one circular CORDIC calculation phase; a third calculation for resolving an angle between a resultant vector and a plane defined by two detection axes, the third calculation utilizing at least two circular CORDIC calculation phases; and a fourth calculation for resolving a length of the resultant vector, the fourth calculation including at least two circular CORDIC calculation phases.

5. The digital circuitry according to claim 1, wherein each of the CORDIC calculations is configured to make a decision on a sign of a subsequent summing operation by comparing a value of a current resultant vector to a zero value.

6. The digital circuitry according to claim 1, wherein each CORDIC calculation is configured to perform two bit shifting operations per CORDIC iteration round.

7. The digital circuitry according to claim 1, wherein the detection signals comprise signals corresponding to acceleration detected along three mutually orthogonal axes, and the CORDIC calculation unit is configured to perform similar calculations using detection signals of any one, two or three of the three axes.

8. The digital circuitry according to claim 1, wherein the digital circuitry further comprises an Arithmetic Logic Unit (ALU) configured to correct sensitivity, offset and rotation of the detection signals, in order to provide corrected detection signals as input for the CORDIC unit.

9. The digital circuitry according to claim 1, wherein the sensor device comprises a MEMS accelerometer.

10. A method for performing calculations for an inclinometer device, the method performed based on detection signals received from a sensor device and the method comprising calculations of inclinometer output values characterizing a resultant vector, wherein the calculations are performed by a CORDIC unit, the method comprising: performing a first calculation for resolving an angle between a resultant vector representing the detection signals and a programmable reference value, the first calculation including at least two hyperbolic CORDIC calculation phases, wherein performing first calculation comprises: determining a first intermediate result corresponding to a length of a projection of the vector on a reference axis times a multiplier representing the change of the length of the vector by using a hyperbolic CORDIC arctangent function; determining a second intermediate result corresponding to a length of the component of the vector orthogonal with the reference axis multiplied with the multiplier representing the change of the length of the vector using a hyperbolic CORDIC arctangent function; and determining an angle between the resultant vector and a calculated axis orthogonal to the reference axis based on the first and the second intermediate results using a circular CORDIC arctangent function.

11. The method according to claim 10, wherein the programmable reference value corresponds to effect of normal gravity along a defined reference axis.

12. The method according to claim 10, wherein a result of the first calculation equals to an angle calculated with an arcsine function between the resultant vector and the programmable reference value, and prior to performing any one of the hyperbolic CORDIC arctangent calculations, the method further comprising: performing at least one pre-rotation for the resultant vector in order to expand a convergence condition of a resulting arcsine function.

13. The method according to claim 10, wherein the method further comprises any one of: performing a second calculation for resolving an angle between a resultant vector or a projection of the resultant vector and an axis in a plane defined by two axes, the second calculation including one circular CORDIC calculation phase; performing a third calculation for resolving an angle between a resultant vector and a plane defined by two detection axes, the third calculation utilizing at least two circular CORDIC calculation phases; and performing a fourth calculation for resolving the length of the resultant vector, the fourth calculation including at least two circular CORDIC calculation phases.

14. The method according to claim 10, wherein each of the CORDIC calculations is configured to make a decision on a sign of a subsequent summing operation by comparing a value of a current resultant vector to a zero value.

15. The method according to claim 10, wherein each of the CORDIC calculations is configured to perform two bit shifting operations per CORDIC iteration round.

16. A method according to claim 10, wherein the detection signals comprise signals corresponding to acceleration detected along three mutually orthogonal axes, and wherein the CORDIC calculation unit is configured to perform similar calculations using detection signals of any one, two or three of the three axes.

17. A method according to claim 10, wherein the method further comprises steps of correcting sensitivity, offset and rotation of the detection signals, the steps of correcting performed by an Arithmetic Logic Unit (ALU), in order to provide corrected detection signals as input for the CORDIC unit.

18. A method according to claim 10, wherein the sensor device comprises a MEMS accelerometer.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In the following the invention will be described in greater detail, in connection with preferred embodiments, with reference to the attached drawings, in which

(2) FIG. 1 illustrates basic concept of an accelerometer.

(3) FIG. 2 illustrates a one dimensional example of a simple microelectromechanical (MEMS) accelerometer.

(4) FIG. 3 illustrates calculation of arctangent function with CORDIC method.

(5) FIG. 4 illustrates traditional calculation of arcsine function with CORDIC method.

(6) FIG. 5 illustrates calculation of an angle in three axis mode

(7) FIG. 6 illustrates calculation of an angle in two axis mode.

(8) FIG. 7 illustrates angle calculation in single axis mode.

(9) FIG. 8 shows a high level schematic view of the data path including an exemplary CORDIC unit.

(10) FIG. 9 illustrates exemplary response functions of an accelerometer.

(11) FIG. 10 illustrates correction of acceleration detection signals.

(12) FIG. 11 represents a schematic view of an exemplary CORDIC unit for implementing calculations for a three-axis inclinometer.

(13) FIG. 12 illustrates error of the length of the resultant vector compared to the vector norm of 1 g.

(14) FIG. 13 illustrates angle errors caused by different arcsine CORDIC implementations.

DETAILED DESCRIPTION

(15) Depending on the design, an accelerometer may provide measurement values representing acceleration in one to three dimensions. The measurement values obtained by the accelerometer in x-, y- and/or z-dimensions define an initial position vector. CORDIC method may be used to define the length and an angle for this position vector, or any resultant vector formed based on one or more of these measurement values.

(16) The FIG. 5 illustrates calculation of an angle , corresponding to the direction of resultant vector g with respect to the detected acceleration of three components a.sub.x, a.sub.y and a.sub.z in direction of the set coordinate axes x, y and z respectively. The algorithm calculates the angle with operation:

(17) $= \arctan \frac{x}{\sqrt{y^{2} + z^{2}}}$
where terms x, y and z correspond to the detected values of a.sub.x, a.sub.y and a.sub.z respectively. Similar notation will be used throughout the description in order to simplify the equations.

(18) FIG. 6 illustrates calculation of an angle , corresponding to the direction of acceleration g when two components of acceleration have been measured, or two components of acceleration are used for calculating the angle . When using this calculation mode, it is assumed that the third axis is in 90 angle (i.e. orthogonal) to the detected resultant vector g. Now the angle is calculated by the algorithm with equation:

(19) $= \arctan \frac{x}{y}$

(20) If the resultant vector g would not be exactly in the xy-plane, this mode will calculate the angle for a projection of the resultant vector g in the xy-plane. In addition to the exemplary xy-plane, similar calculation may be performed in yz-plane or in xz-plane, when replacing x and y with y and z or x and z respectively.

(21) FIG. 7 illustrates angle calculation in single axis mode. Now, the angle of each axis is calculated with respect to a reference vector (one_g) corresponding to effect of normal gravitation on a device otherwise in rest. Such reference vector may also be called as a reference gravitation vector. In this case it is assumed that each axis has been calibrated so that the 1 g value (one_g) is known for each axis. The reference vector, or the reference values corresponding to the reference vector can be programmable, so that other reference vector can be chosen to fine tune the device instead of the exemplary one_g gravitation reference vector. The term programmable indicates that although the reference vector is preferably defined during calibration of the device, the reference vector may even be made programmable by a user of the sensor device. However, the latter is an optional feature, and defining the reference vector by the manufacturer for instance during calibration of the device shall be considered fulfilling the programmability characteristic. Then the angle between each individual axis and the exemplary one_g reference vector may be calculated like:

(22) $= \arcsin \frac{x}{one_g}$

(23) When calculating angle of y or z, x in the above equation will be replaced with the respective value of y or z, and the one_g reference value will be selected accordingly for the respective axis. Similar calculation may be performed for any selected reference vector by replacing the one_g reference value.

(24) In addition to the three modes for calculating angle , length of the resultant vector g may be calculated for any and each of the three modes with equation:
length(g)={square root over (x.sup.2+y.sup.2+z.sup.2)}

(25) Calculated length of the resultant vector (length(g)) may further be compared to a reference value, such as length of the selected reference vector (i.e. one_g).

(26) The traditional methods of using CORDIC for calculating angle and length of a resultant vector (a position vector) was explained earlier in relation to FIGS. 2 and 3. An improved method to calculate the arcsine used at least in the single axis mode will be described next, avoiding the problems caused by double iteration, target level shifting and scaling the resultant vector length needed traditionally for arcsine calculation with CORDIC. In the improved method, the arcsine function is calculated by utilizing hyperbolic functions, which may be calculated with CORDIC method with equations:
x.sub.i+1=x.sub.i2.sup.i.Math.y.sub.i
y.sub.i+1=y.sub.i2.sup.i.Math.x.sub.i
.sub.i+1=.sub.i.sub.i.sub._.sub.ROMH
where i=1, 2, 3 . . . , n. Rotating the x and y values is always made in mutually same direction, whereas in calculation of arctangent function direction of rotation would vary. Using this hyperbolic method for performing arcsine calculation allows comparison of the current value with zero value, in other words a sign detection operation. These comparisons are made for the intermediate results of the respective accumulators (118, 120 in FIG. 11). By enabling use of such relatively simple comparison operation without any need for level shifting risk of errors in the decisions is reduced in comparison with traditional arcsine calculation. Value of x approaches value K.sub.H{square root over ((x.sup.2y.sup.2))} and resultant angle approaches value arctanh(x/y). When calculating hyperbolic functions, the resultant vector length gets shorter with each iteration by multiplier
.sub.i=0.sup.n{square root over ((12.sup.2i))}0.8281,n.fwdarw.

(27) Condition for convergence of hyperbolic series causes a requirement that rotations i=4, 13, 40 . . . need to be made twice. In addition, hyperbolic tangent function has a convergence condition value 1.1182, which corresponds to an angle limit of approximately 64.07. In other words, a CORDIC unit utilizing an uncorrected hyperbolic tangent function will only be able to calculate angles that are less than 64.07. This limitation may be circumvented by performing one or more so called pre-rotations. For example, two pre-rotations may be performed for the resultant vector before starting the actual hyperbolic CORDIC method (i=1, 0):
x.sub.i+1=x.sub.i(12.sup.i2).Math.y.sub.i
y.sub.i+1=y.sub.i(12.sup.i2).Math.x.sub.i

(28) These pre-rotations may be performed with the same circuitry that performs the hyperbolic functions, if the expressions in parenthesis are opened in order to generate separate rotations (i=3, 2):
(12.sup.i2).Math.x.sub.i=x.sub.i2.sup.ix.sub.i
(12.sup.i2).Math.y.sub.i=y.sub.i2.sup.iy.sub.i

(29) With the exemplary two pre-rotations, the convergence condition of the resulting arcsine calculation expands up to approximately 88.5, which is sufficient for the inclinometer application. Convergence condition of the calculation refers to the capability of the calculation to converge to a defined result, wherein the angle indicates the maximum resultant angle that may be calculated with acceptable accuracy. For the hyperbolic tangent function, increasing the number of pre-rotations brings the convergence condition closer to 90. Arcsine function is solved by using the hyperbolic tangent function first to solve the length of the unknown edge of the triangle with Pythagorean method {square root over (one_g.sup.2x.sup.2)}, and then using this result for solving the respective resultant angle () with traditional circular CORDIC arctangent operation:

(30) $= \arctan \frac{x}{\sqrt{{one_g}^{2} - x^{2}}}$

(31) During the hyperbolic operation, the amount of bit shifting in the bit shifting operation as specified for hyperbolic operation may be obtained from a static table. Such static table may be for instance stored in a Read Only Memory (ROM). In traditional hyperbolic CORDIC operations amount of shifting is obtained directly from the number of the iteration. As an alternative for using a static table, a counter may be used that has been configured to repeat certain values according to the known sequence.

(32) Calculating the length of the resultant vector utilizes capability of arctangent operation, where x approaches value K.sub.C{square root over (x.sup.2+y.sup.2)}. In order to calculate the length of the resultant vector in a three axis coordinate system, the calculation of the root expression is divided into three phases:
l.sub.1=K.sub.C{square root over (x.sup.2+y.sup.2)}
l.sub.2=K.sub.Cz
l.sub.3=K.sub.C{square root over (l.sub.1.sup.2+l.sub.2.sup.2)}=K.sub.C.sup.2{square root over (x.sup.2+y.sup.2+z.sup.2)}

(33) In order to remove multiplier K.sub.C.sup.2, a division is performed using a linear CORDIC operation defined by:
x.sub.i+1=x.sub.i
y.sub.i+1=y.sub.i2.sup.i.Math.x.sub.i
.sub.i+1=.sub.i2.sup.i.Math.d.sub.i

(34) Now the angle .sub.i approaches value d.sub.0.Math.y.sub.0/x.sub.0. By placing a constant value K.sub.C.sup.2 scaled with a suitable power of two as divisor x.sub.0, and using the selected reference value as the initial value for the angle d.sub.0, the resultant vector length will be received directly in relation to the selected reference vector length. As the linear CORDIC calculation does not need an x-accumulator or a shifter, it is used as an accumulator for angle value and/or as a shifter in the operation mode for calculating the length of the resultant vector. A power of two may be selected for scaling factor in order to avoid the last multiplication. The output will be of form:
length=.sub.n,
where .sub.n represents the resultant angle after n iterations.

(35) Calculation of the length of the resultant vector is a useful method for diagnostics purpose. It may be used for example for checking validity of the angle calculations. For an accelerometer that is in rest, i.e. not subject to any other acceleration than the gravitation force, the length of the resultant vector should be equal to acceleration caused by gravity, i.e. the set value of one_g for each axis. If this is not the case, there is some error in the CORDIC calculations, which may be caused for example by inadequate corrections made for offset, sensitivity and/or alignment.

(36) FIG. 8 shows a high level schematic view of the data path including an exemplary CORDIC unit (CORDIC) configured to work with an accelerometer sensor device (not shown) capable of detecting acceleration along three different and preferably but not necessarily orthogonal axes. Input from the sensor device received over an electrical interface in form of electrical detection signals (acc.sub.x, acc.sub.y, acc.sub.z) corresponding to the vector components of the total acceleration detected by the accelerometer device along the three different detection axes. The incoming detection signals (acc.sub.x, acc.sub.y, acc.sub.z) can be corrected for achieving higher accuracy with an Arithmetic Logic Unit (ALU), which may perform corrections on sensitivity, offset and cross axis alignment, which corrections will be described in relation to FIGS. 9 and 10, so that the CORDIC unit (CORDIC) receives corrected detection signals (x.sub.in, y.sub.in, z.sub.in) in its inputs. Multiplexers (MUXA, MUX B) are used for making selection on input signals (A.sub.in, B.sub.in) that are to be fed into the CORDIC unit (CORDIC). The multiplexers (MUX A, MUX B) are controlled by a controller (CTRL), which may comprise for example a state machine or a sequencer. This controller (CTRL) controls the selection of input signals (A.sub.in, B.sub.in) by the multiplexers (MUX A, MUX B) into the CORDIC unit (CORDIC). Possible input signals (A.sub.in, B.sub.in) comprise the corrected detection signals (x.sub.in, y.sub.in, z.sub.in), results of previous calculation phases available in feedback registers (reg.sub.0, reg.sub.1) and preset reference values (zero, one_g, K.sub.C.sup.2). Selections of the input signals (A.sub.in, B.sub.in) to the CORDIC calculation unit (CORDIC) depend on the next CORDIC calculation phase to be performed by the CORDIC unit (CORDIC).

(37) The CORDIC unit (CORDIC) performs the calculation operations and as a result, output is received which may represent an angle (), an intermediate result of an iteration round (A.sub.i+1) or an intermediate calculation phase result from a finalized CORDIC calculation phase, after the preset number of n iterations, for storing into a feedback register (reg.sub.0, reg.sub.1).

(38) FIGS. 9 and 10 illustrate compensation of non-idealities in case of a three axis accelerometer.

(39) FIG. 9 illustrates an exemplary response functions of an accelerometer. Detected acceleration is illustrated on the horizontal axis (acc), while the output of the accelerometer is illustrated on the vertical axis (out). An intended linear response function (110) is presented with a line having a 45 slope with respect to both axes (acc, out). Error of sensitivity is illustrated with dashed line (105), having a slope that differs from the intended response function (110) slope. Other type of error in sensitivity could be that the sensitivity would change over the scale, in other words, the response function would not be a straight line, but a curve. Error in offset is illustrated with dotted line (106) which is not crossing the axes in the origin. Such offset error would mean that the sensor output would show a non-zero value even if the acceleration was zero.

(40) Due to the non-linear characteristic of the circular and hyperbolic CORDIC calculation operations in the CORDIC unit, compensations of non-idealities shall be performed before CORDIC calculation is performed. This is because after a non-linear operation, it would not be possible to correct the error caused in the result due to the errors input with reasonable effort.

(41) Two main phases may be recognized for the non-ideality compensation: calibration and correction. Calibration refers to calibration or correction performed as part of manufacturing process of the device marked with phases 100x, 100y, 100z. Calibration is performed for each applicable axis. After installation and/or during use of the device, both sensitivity correction (101x, 10y, 101z) and offset correction (102x, 102y, 102z) is provided for each applicable axisin this example for x, y and z-axis. Further, a rotation matrix is provided in rotation correction (103) for further correcting of the directions of the acceleration axes. The corrections (101x, 101y, 101z, 102x, 102y, 102z, 103) may be used to compensate for example errors caused by manufacturing and/or installation process or drifting of the measured acceleration values caused over time.

(42) In the exemplary embodiment presented in FIG. 10, an initial acceleration calibration (100x, 100y, 100z) is performed first for each detection axis. This acceleration calibration refers to a calibration performed for the product as part of the manufacturing process. Acceleration calibration (100x, 100y, 100z) includes at least calibrating sensitivity and offset of the device.

(43) Sensitivity calibration means that the device is initially calibrated so that the sensitivity of the device, i.e. the slope of the linear output response versus the detected acceleration is calibrated along each relevant axis so that the response is directly proportional to the amount of acceleration in the direction of each respective axis. Calibrating offset means that the output value provided by the accelerometer device is calibrated so that the output shows zero value at zero acceleration on each applicable measurement axis. Acceleration calibration (100x, 100y, 100z) may be performed simultaneously for all axes, or it may be performed one axis at a time. Acceleration calibration values may be stored in a memory and same stored acceleration calibration values may be used for subsequently, so that adjustment of acceleration calibration values may be performed just once. Acceleration calibration (100x, 100y, 100z) may be part of the manufacturing process so that in practice it is typically only performed once for each device.

(44) Although the accelerometer is now appropriately acceleration calibrated (100x, 100y, 100z) during the manufacturing process, the calibration may not last throughout the lifetime of the device. For example, when a semiconductor device is installed by soldering, it experiences typically a high temperature shock during a soldering process, which may cause changes in the performance of the device. Another example of cause of need for further adjusting the operational settings of the device is that the device may be installed in a position which is not fully aligned with the originally calibrated axes. Further, aging may cause deviation of the operational status from the original calibration. A person familiar with the art even knows further reasons causing need for correcting the operation of an accelerometer.

(45) In order to set the operation state of an accelerometer to the desired accuracy, further correction may be needed for operation settings. Sensitivity, offset and axis rotation may be corrected.

(46) As described above, sensitivity refers to the amount of displacement caused by external acceleration and thus to the output signal from the sensor device caused by a detected amount of acceleration. Adjusting sensitivity refers to setting the slope of the preferably essentially linear response function of the device to a wanted value. Device sensitivity may be corrected for each axis separately. Sensitivity correction (101x, 101y, 101z) may be performed either manually by a user or automatically. For a multi-axis sensor device having more than one detection axis, sensitivity may be corrected simultaneously for at least two axes, or sensitivity correction (101x, 101y, 101z) may be performed to each axis one by one, at different times.

(47) Correcting offset means setting the detected output value to show zero when the detected acceleration along the respective axis is zero. Offset may be corrected for each axis separately. Offset correction may be performed either manually by a user or automatically. For a multi-axis sensor device having more than one detection axis, offset may be corrected (102x, 102y, 102z) simultaneously for at least two axis, or offset correction (102x, 102y, 102z) may be performed to each axis one by one, at different times.

(48) It should be noted that although FIG. 10 shows the sensitivity correction (101x, 101y, 101z) and offset correction (102x, 102y, 102z) in a particular order, these correction phases may be performed in any order without departing from the scope.

(49) When all applicable axes have been scaled by the initial acceleration calibration (100x, 100y, 100z), and later sensitivity and offset corrections (101x, 101y, 101z, 102x, 102y, 102z) as described above, rotation of the axes may be adjusted in rotation correction (103) using a rotation matrix. Rotation correction (103) of the axes with the rotation matrix means that the orientation of the coordinates defined by the axes is aligned with the environment. By rotating the axes with the rotation matrix it can be ensured that the device measures acceleration along axes that are aligned exactly as intended, even if there would be some misalignment initially caused for example by errors in installation, or some long term drift caused to the alignment. If for some reason the detection axes of the sensor device would not initially be fully orthogonal with each other, the rotation matrix may even be used for correcting relative alignment of the axes, so that they become fully orthogonal. Use of rotation matrix for correcting the alignment of the axes is known to a person skilled in the art.

(50) After the output detection signals (acc.sub.x, acc.sub.y, acc.sub.z) of the accelerometer are fully scaled, corrected and aligned into corrected detection signals (x.sub.in, y.sub.in, z.sub.in), these are ready to be used for detecting alignment, and the CORDIC unit may be used for calculation of the angle values. While the circular and hyperbolic CORDIC methods for calculating angles are not a linear methods, it is necessary to make any needed corrections to sensitivity, offset and/or alignment of axes prior to calculation of angle values. Due to the nonlinearity of the circular and hyperbolic CORDIC calculation processes, any errors caused in the result due to errors in input values for the CORDIC calculation cannot be recognized and subtracted after the CORDIC (angle) calculation.

(51) Although we have described here a three axis accelerometer, similar process may be applied to a one or two axis accelerometer. The relative order of sensitivity correction (101x, 101y, 101z) and offset correction (102x, 102y, 102z) in each direction may be performed either in parallel, or in series, one axis at a time without departing from the scope. Rotation correction (103) may preferably be performed only after all applicable axes have been sensitivity and offset corrected.

(52) FIG. 11 represents a schematic view of an exemplary CORDIC unit for implementing calculations for a three-axis inclinometer. This unit has two iteration circuitries (111, 112) in parallel, both comprising a bit shifting circuitry (SHIFT A, SHIFT B) and a summing circuitry (ADD A, ADD B) so that it is capable of performing two parallel CORDIC bit shifting and summing operations simultaneously. First multiplexers (MUX.sub.IA, MUX.sub.IB) are used for selecting appropriate inputs for the summing circuitries (ADD A, ADD B) so that when a new iteration round is initiated, these inputs correspond to the input signals (A.sub.in, B.sub.in), and during iteration rounds the signal to be summed is the previous intermediate result (A.sub.i, B.sub.i). Different calculation modes are implemented by feeding the input signals or intermediate results (A.sub.in, B.sub.in; A.sub.i, B.sub.i) to the respective summing circuitries (ADD A, ADDB) from CORDIC unit input ports and controlling the shifting operations performed in the respective bit shifting circuitries (SHIFT A, SHIFT B) through a control input fed from second multiplexers (MUX.sub.SA, MUX.sub.SB). Iteration input is used for normal, circular CORDIC calculations, where the iteration sequence is a simple linear sequence (1, 2, 3, 4, 5, 6 . . . , n). Hyperbolic functions are controlled with an input from a hyperbolic rotation table stored for example in a ROM memory, indicated with reference Hyperbolic rotation ROM. As described earlier, in this hyperbolic iteration some iteration steps are duplicated, and the sequence may be for example (i=1, 2, 3, 4, 4, 5, . . . ), meaning that rotations i=4, 13, 40, . . . will be repeated in order to make the hyperbolic function converge. The overall sequence for bit shifting in Hyperbolic rotation, including two pre-rotations in the sequence would start with a sequence like (i=3, 2, 1, 2, 3, 4, 4, . . . ). Value 1 is fed into adder (ADD A) from MUX.sub.SA in order to perform a simple division operation, such as phase 4 of the resultant length calculation as will be shown later in Table 1.

(53) Third multiplexers (MUX.sub.ITA, MUX.sub.ITB) are used to control the selection of the signal that is input to the respective bit shifting circuitries (SHIFT A, SHIFT B) for bit shifting operation. Depending on the calculation operation, this signal may be one of the result of previous iteration (A.sub.i, B.sub.i), temporary results of pre-rotations (A.sub.pre, B.sub.pre). A pre-rotation phase needs two intermediate results, namely A.sub.i and 3/4*A.sub.i (or B.sub.i and 3/4*B.sub.i) during the same iteration round. While the pre-rotation phase does not need to accumulate angle values in the angle accumulator (117), it may temporarily be used as a register for the temporary result of pre-rotation A (A.sub.pre) received from the first iteration circuitry (111). Another accumulator (119) is used as register for the temporary result of pre-rotation B (B.sub.pre) from the second iteration circuitry (112). By re-using the angle accumulator (117) for this purpose, there is no need for adding another register, and therefore size of circuitry does not need to be increased for implementing the needed register for storing the temporary result of pre-rotation A (A.sub.pre). Alternatively, a separate register may be provided, which would, however, slightly increase size of the circuitry.

(54) The decision about whether the next intermediate iteration value (A.sub.i+1, B.sub.i+1) will be summed or subtracted is made based on the sign given by the respective sign defining circuitry (SIGN(A.sub.i+1), SIGN(B.sub.i+1)). Sign defining circuitries (SIGN(A.sub.i+1), SIGN(B.sub.i+1)) detect sign of the result obtained from the previous iteration round in order to decide whether next angle value will be added or deducted.

(55) Demultiplexers (DEMUX A, DEMUX B) are used for feeding the result values received from the respective summing circuitry (ADD A, ADD B) into different output/feedback branches. Demultiplexer DEMUX A is used to control whether the result value received from the first iteration circuitry (111) at each clock cycle is to be stored to an accumulator (118) or to an input of an arctangent circuitry (113) configured to perform arctangent calculation. Demultiplexer DEMUX B controls selection feeding the result value received from the second iteration circuitry (112) at each clock cycle in either of the two accumulators (119, 120), so that the result value is available for feeding back to the input of either of the iteration circuitries (111, 112) as needed for next iteration. Accumulator 119 is used during pre-rotations for storing the temporary result of pre-rotation B (B.sub.pre), while accumulator 120 is used for storing intermediate value (B.sub.i) during rotation operations. Multiplexer MUX.sub.AB is used for selecting an input signal (A.sub.i) for the second iteration circuitry (112) between the original input signal (A.sub.in) and the result of the previous iteration round (A.sub.i) performed by the first iteration circuitry (111). The input signal (A.sub.in) is used when a new calculation phase is initiated and the result of the previous iteration round (A.sub.i) is used during iteration rounds.

(56) Accumulators (z.sup.1) act both as intermediate storage and as delay for the calculation results. They are used for storing the result of the current iteration result (A.sub.i+1, B.sub.i+1) and the contents of the accumulators (z.sup.1) may be read as the output of the CORDIC unit ( Out, A.sub.i+1 Out). The contents of the accumulators may also be utilized as input for the subsequent iteration, so that the previous iteration result (A.sub.i, B.sub.i) stored in an accumulator (z.sup.1) may be selected as input for the summation (ADD A, ADD B) and/or bit shifting operations (SHIFT A, SHIFT B) on next iteration round. Results of the performed CORDIC calculation phases may be obtained either from the A-accumulator (118) or from the angle accumulator (117). These results may be the results of calculation phases that will be stored in one of the registers (reg.sub.0, reg.sub.1), or final results (,length) of calculations according to the specific mode.

(57) Arctangent circuitry (113) comprises a summing circuitry (ADD), two multiplexers (MUX.sub.ATA1, MUX.sub.ATA2), and the angle accumulator (117). Arctangent circuitry (113) uses a table (arctan ROM) containing present elementary angle values (.sub.i.sub._.sub.ROM) for calculating the sum of angles during the iteration, which is then provided as a resulting angle value ( Out) from the accumulator (117) of the arctangent circuitry (113). The decision whether the elementary angle value (.sub.i.sub._.sub.ROM) is to be added or subtracted from the previous sum is controlled by the sign defining circuitry (SIGN(A.sub.i+1)). During pre-rotation, the angle accumulator (117) of the arctangent circuitry (113) may also be used for intermediate storage for temporary result of pre-rotation A (A.sub.pre), which may be then fed as input value in the second iteration circuitry (112). When the angle accumulator (117) is utilized for storing the angle value during iteration, the current sum of angle values (.sub.i) will be overwritten by the next sum (.sub.i+1) in the angle accumulator (117) of the arctangent circuitry during next clock period.

(58) CORDIC calculation typically requires multiple iteration rounds. Thus, applicability of CORDIC method in a particular application requires that there is sufficient time to perform the needed calculations. In inclinometer use, timing is typically not a limiting factor: an accelerometer device provides samples at relatively long intervals when compared to clock frequency used for example in an ASIC circuitry performing CORDIC calculations.

(59) Number of iteration rounds used for each calculation mode may be selected based on the type of angle calculation. In an exemplary embodiment, 16 iterations are performed for a normal angle calculation (arc tan), and 20 iterations are performed for hyperbolic function calculation (sin h). Calculation of the resultant vector length requires 2 to 4 additional operation phases after angle calculation, depending on the mode.

(60) Table 1 below summarizes the phases for different modes of operation of the CORDIC unit. It can be noted that in three axes mode, three calculation phases are needed to complete the angle calculations in the CORDIC unit. With the term phase we refer to a calculation of an intermediate result or a final output value, the calculation comprising an iteration sequence of a predefined number of elementary rotations performed by the CORDIC unit. Each iteration step requires a period of time corresponding to one clock cycle. The same sequence of calculation phases for a calculation mode may be repeated for each different axis of calculation, always starting from a selection and order of input values suitable for the specific calculation mode and axis. For example, if three axes are used for calculations as in the exemplary CORDIC unit, the sequence of calculation phases may be repeated three times in order to receive results for each one of the three axes.

(61) In two axes mode just one calculation phase is needed to complete the angle calculation, as the result may be directly obtained with arctangent calculation. In one axis mode, three calculation phases are needed to complete the angle calculations. For resultant vector length calculations, four calculation phases are needed to achieve the intended result. Is should be noticed that order of performance of some of the calculation phases may be changed without changing the result of the calculations. Such alternative orders will be mentioned later in the description.

(62) TABLE-US-00001 Mode Phase 1 Phase 2 Phase 3 Phase 4 $= \arctan \frac{x}{\sqrt{y^{2} + z^{2}}}$ reg.sub.0 = K.sub.c .Math. x.sub.in reg.sub.1 = K.sub.c .Math. {square root over (y.sub.in.sup.2+z.sub.in.sup.2)} $= \arctan \frac{{reg}_{0}}{{reg}_{1}}$ $= \arctan \frac{x}{y}$ 0 $= \arctan \frac{x_{in}}{y_{in}}$ $= \arcsin \frac{x}{one_g}$ reg.sub.0 = K.sub.H .Math. x.sub.in reg.sub.1 = K.sub.H .Math. {square root over (one_g.sub.in.sup.2x.sub.in.sup.2)} $= \arctan \frac{{reg}_{0}}{{reg}_{1}}$ len = {square root over (x.sup.2+y.sup.2+z.sup.2)} reg.sub.0 = K.sub.C .Math. x.sub.in reg.sub.1 = K.sub.C .Math. {square root over (y.sub.in.sup.2+z.sub.in.sup.2)} reg.sub.0 = K.sub.C .Math. {square root over (reg.sub.0.sup.2+reg.sub.1.sup.2)} $= \frac{{reg}_{0}}{K_{C}^{2} .Math. scale}$

(63) All operations described above may be implemented with a 25-bit CORDIC unit, such as the one described in connection to FIGS. 8 and 11.

(64) In the three axis mode represented by

(65) $= \arctan \frac{x}{\sqrt{y^{2} + z^{2}}}$
mode in the presented table 1, angle is calculated with respect to three coordinate axes. Phase 1 provides a first intermediate calculation phase result corresponding to a projection of the vector in x-axis direction times the multiplier (K.sub.C) representing the change of the length of the resultant vector. This value is stored in a first register (reg.sub.0) for subsequent use. Phase 2 provides a second intermediate calculation phase result corresponding to a projection of the vector in yz-plane, i.e. the plane defined by the y- and z-axes, also multiplied with the multiplier (K.sub.C) representing the change of the length of the resultant vector. As long as the number of iterations is equal in phase 1 and phase 2, these multipliers (K.sub.C) are equal. The second intermediate calculation phase result is stored in a second register (reg.sub.1) for subsequent use. While the used CORDIC calculation algorithm and the number of iterations are the same, the values of the multipliers (K.sub.C) received from Phase 1 and Phase 2 are equal, and will thus cancel each other in division operation. It should also be noticed that the order of performing Phase 1 and Phase 2 may be changed, as these two calculation phases are independent of each other. In phase 3, the resulting angle is calculated using the first and second intermediate calculation phase results stored in registers (reg.sub.0, reg.sub.1) as input values for the CORDIC arctangent calculation phase performed mainly in the arctangent circuitry (113). It can be noticed that the multipliers (K.sub.C) now have become cancelled in the division operation in phase 3, and the arctangent function gives the actual value of the detected angle ().

(66) In the two axis mode represented by

(67) $= \arctan \frac{x}{y}$
in the presented table, angle is calculated with respect to two coordinate axes. This is the simplest mode in view of CORDIC calculation: just a single phase of calculations is needed for each direction with respect to a plane formed by two axes. The corrected pair of input values (x.sub.in, y.sub.in; y.sub.in, z.sub.in; x.sub.in, z.sub.in) are used as input for calculation, and after the set number of iteration rounds, the result is provided as output from the CORDIC unit.

(68) In the one axis mode represented by

(69) $= \arcsin \frac{x}{one_g}$
in the presented table, angle is calculated with respect to reference value one_g, corresponding to situation where the device is in rest so that only gravity affects it. Instead of one_g reference values, any other reference values could be programmed as a reference values for each axis. Preferably, the reference value for an axis corresponds to length of a component of the same reference vector along the respective axis. Phase 1 provides a first intermediate calculation phase result for length of a projection of the vector in x-axis (or any chosen reference axis) direction times a multiplier (K.sub.H) representing the change of the length of the vector in this hyperbolic CORDIC calculation. The first intermediate calculation phase result from the phase 1 hyperbolic CORDIC calculation is stored in a first register (reg.sub.0) for subsequent use. Phase 2 provides a second intermediate calculation phase result representing length of the component of the vector orthogonal with the x-axis (or any chosen reference axis) multiplied with the multiplier (K.sub.H) representing the change of the length of the vector in this hyperbolic CORDIC calculation. This second intermediate calculation phase result of this second hyperbolic CORDIC calculation is stored in a second register (reg.sub.1) for subsequent use. While the used CORDIC calculation algorithm and the number of iterations are the same for both phases, the values of the multipliers (K.sub.H) received in Phase 1 and Phase 2 are equal. It should also be noticed that the order of performing Phase 1 and Phase 2 may be changed, as these two calculation phases are independent of each other. In phase 3, the resulting angle (), corresponding to the angle between the vector and the calculated axis orthogonal to the x-axis (or any other chosen reference axis) is calculated using the first and the second intermediate calculation phase results stored in first and second registers (reg.sub.0, reg.sub.1) during phase 1 and phase 2 as input values for the normal circular arctangent CORDIC calculation. It can be noticed that the multipliers (K.sub.H) now become cancelled in the division operation in phase 3, and the final result is the actual angle value (). When all sides of the triangle of the vectors of the right triangle are now known, as well as two angles (one of which is a right angle), the third angle is also known.

(70) The fourth mode represents calculation of length of the resultant vector according to len={square root over (x.sup.2+y.sup.2+z.sup.2)}. This calculation requires a total of four phases to be completed. Phase 1 provides a first intermediate calculation phase result for projection of the vector in x-axis direction times the multiplier (K.sub.C) representing the change of the length of the resultant vector. This first intermediate calculation phase result is stored in a first register (reg.sub.0) for subsequent use. Phase 2 provides a second intermediate calculation phase result for projection of the vector in yz-plane, i.e. the plane defined by the y- and z-axes, also multiplied with the multiplier representing the change of the length of the resultant vector (K.sub.C). This second intermediate calculation phase result is stored in a second register (reg.sub.1) for subsequent use. While the used CORDIC calculation algorithm and the number of iterations are the same, the values of the multipliers (K.sub.C) received in Phase 1 and Phase 2 are equal. It should also be noticed that the order of performing Phase 1 and Phase 2 may be changed, as these two calculation phases are independent of each other. In Phase 3, a third intermediate calculation phase result is calculated representing the total length of the vector multiplied with another instance of the same multiplier (K.sub.C). The third intermediate calculation phase result in phase 3 may be written in the first register (reg.sub.0) which was earlier used for storing the first intermediate calculation phase result received from Phase 1, since the old result is not needed any more. In the final phase, the third intermediate calculation phase result stored in the first register (reg.sub.0) and second intermediate calculation phase result stored in the second register (reg.sub.1) are used for calculating the final result, corresponding to the length of the resultant vector. In an alternative embodiment, a separate register may be provided for the third intermediate calculation phase result, with cost of need for a register, which increases slightly the total size of the circuitry. The calculation of length of the resultant vector may be used for instance for error detection in the system. When the length of the resultant vector is calculated for a device that is not moving nor tilted in any way, the length of the resultant vector should be equal to the one_g reference. If the length of the resultant vector would deviate from this value, error has occurred during the process, either in the calculations, in the detection element or in calibration and/or correction in the ALU unit.

(71) The user of the device may choose the mode(s) that are calculated for each set of samples. In a typical application, just one mode for calculations is selected, and the corresponding phases are executed in order to calculate the intended result values for each relevant axis one by one. However, the system does not limit the amount of result values (angle, length) calculated. If time allows, the system may be used to calculate all possible result values available or a selection of result values from more than one mode.

(72) In any mode, calculation may be performed one phase at a time for all relevant axes, or the whole calculation process may be first completed for one axis before initiating calculations for the next axis. Calculating results for one axis and one mode at a time allows minimizing the amount of needed storage for intermediate calculation phase results, thus optimizing the size of the circuitry.

(73) An example of how total time required for the calculations required to perform calculation of angle values for three different axes one by one in the exemplary CORDIC unit may be illustrated with simple calculation. Let's assume we wish to obtain results for all three axes in

(74) $= \arcsin \frac{x}{one_g}$
mode. With one CORDIC unit implementation as described in FIGS. 8 and 11, each of the 3 different directions

(75) $(_{x} = \arcsin \frac{x}{one_g},_{y} = \arcsin \frac{y}{one_g},_{z} = \arcsin \frac{z}{one_g})$
are calculated separately, and each calculation requires 3 phases. If each of the three phases would be chosen to be done with 20 iterations, total time required to perform the calculations would be 3*3*20=180 clock cycles. If the number of iterations would be defined to be less than 20 for the final arctangent calculation phase, for example 16 iterations, the calculation would require a total of 3*(2*20+16)=168 clock cycles.

(76) In angle calculations with a CORDIC unit described in FIGS. 8 and 11, by using 16 iterations for normal arctangent angle calculations and 20 iterations for hyperbolic function calculations, an error margin of less than 0.01 is achieved. Including the calculation of the resultant vector length in the same CORDIC unit will increase the size of the device only with circuitry required to perform the final division phase, namely multiplexers MUX.sub.ATA2 ja MUX.sub.AB, and a couple of additional inputs (A.sub.pre, B.sub.pre) to existing multiplexers to facilitate the pre-rotation operation.

(77) FIG. 13 illustrates angle errors caused by different arcsine CORDIC implementations. Thin line (131) presents result of a simple arcsine calculation using CORDIC without corrections. Although this thin line (131) mostly follow the zero error line accurately, it can be noticed that even rather large error spikes in varying directions with magnitude order of 5 degrees appear along the scale, and when the angle is more than about 80, the error becomes even larger. Thus, simple arcsine calculation like this is not feasible for accurate acceleration devices. Dashed line (132) illustrates error of a double iterations arcsine calculation with a prior art CORDIC unit. In this calculation, the number of spikes have been reduced, but the simulation shows significant error with large resultant angles. The resulting hyperbolic arcsine function calculated according to the current invention, using two pre-rotations, is marked with a thick, solid line. It may be noticed that this function provides very linear result up to about 88 angles, which is sufficient for the intended use of the CORDIC unit with an accelerometer for implementing an inclinometer. It should be noticed, that if the angle of the resultant vector is known to be relatively small, hyperbolic function may be simplified so that it is calculated without pre-rotations, or with just a single pre-rotation. On the other hand, if angles close to 90 should be reliably detected, more than two pre-rotations may be used, thus expanding the convergence condition of the calculation function closer to 90.

(78) FIG. 12 illustrates error of the length of the resultant vector compared to the norm of one_g with calculation performed with the method as described above. With the exemplary 25 bit CORDIC unit, the resultant vector length may be calculated with error margin that is less than 0.0002*one_g, i.e. less than 0.02%.

(79) It is apparent to a person skilled in the art that as technology advanced, the basic idea of the invention can be implemented in various ways. The invention and its embodiments are therefore not restricted to the above examples, but they may vary within the scope of the claims.

Digital circuitry and method for calculating inclinometer angles

Assignee

Inventors

Cpc classification

Classification Explorer

G06F7/4818

PHYSICS

Classification Explorer

G01C9/00

PHYSICS

Classification Explorer

G01P15/08

PHYSICS

Classification Explorer

G01C9/02

PHYSICS

International classification

Classification Explorer

G01C9/02

PHYSICS

Classification Explorer

G06F7/48

PHYSICS

Classification Explorer

G01C9/00

PHYSICS

Classification Explorer

G01P15/08

PHYSICS

Abstract

Claims

Description