LP-311 Remove basic/advanced stabilization tab auto-switch (autotune/txpid lock issues)
[librepilot.git] / ground / gcs / src / libs / eigen / blas / srotmg.f
blob7b3bd427287d95288cb8c8c24437e33485b77160
1 SUBROUTINE SROTMG(SD1,SD2,SX1,SY1,SPARAM)
2 * .. Scalar Arguments ..
3 REAL SD1,SD2,SX1,SY1
4 * ..
5 * .. Array Arguments ..
6 REAL SPARAM(5)
7 * ..
9 * Purpose
10 * =======
12 * CONSTRUCT THE MODIFIED GIVENS TRANSFORMATION MATRIX H WHICH ZEROS
13 * THE SECOND COMPONENT OF THE 2-VECTOR (SQRT(SD1)*SX1,SQRT(SD2)*
14 * SY2)**T.
15 * WITH SPARAM(1)=SFLAG, H HAS ONE OF THE FOLLOWING FORMS..
17 * SFLAG=-1.E0 SFLAG=0.E0 SFLAG=1.E0 SFLAG=-2.E0
19 * (SH11 SH12) (1.E0 SH12) (SH11 1.E0) (1.E0 0.E0)
20 * H=( ) ( ) ( ) ( )
21 * (SH21 SH22), (SH21 1.E0), (-1.E0 SH22), (0.E0 1.E0).
22 * LOCATIONS 2-4 OF SPARAM CONTAIN SH11,SH21,SH12, AND SH22
23 * RESPECTIVELY. (VALUES OF 1.E0, -1.E0, OR 0.E0 IMPLIED BY THE
24 * VALUE OF SPARAM(1) ARE NOT STORED IN SPARAM.)
26 * THE VALUES OF GAMSQ AND RGAMSQ SET IN THE DATA STATEMENT MAY BE
27 * INEXACT. THIS IS OK AS THEY ARE ONLY USED FOR TESTING THE SIZE
28 * OF SD1 AND SD2. ALL ACTUAL SCALING OF DATA IS DONE USING GAM.
31 * Arguments
32 * =========
35 * SD1 (input/output) REAL
37 * SD2 (input/output) REAL
39 * SX1 (input/output) REAL
41 * SY1 (input) REAL
44 * SPARAM (input/output) REAL array, dimension 5
45 * SPARAM(1)=SFLAG
46 * SPARAM(2)=SH11
47 * SPARAM(3)=SH21
48 * SPARAM(4)=SH12
49 * SPARAM(5)=SH22
51 * =====================================================================
53 * .. Local Scalars ..
54 REAL GAM,GAMSQ,ONE,RGAMSQ,SFLAG,SH11,SH12,SH21,SH22,SP1,SP2,SQ1,
55 + SQ2,STEMP,SU,TWO,ZERO
56 INTEGER IGO
57 * ..
58 * .. Intrinsic Functions ..
59 INTRINSIC ABS
60 * ..
61 * .. Data statements ..
63 DATA ZERO,ONE,TWO/0.E0,1.E0,2.E0/
64 DATA GAM,GAMSQ,RGAMSQ/4096.E0,1.67772E7,5.96046E-8/
65 * ..
67 IF (.NOT.SD1.LT.ZERO) GO TO 10
68 * GO ZERO-H-D-AND-SX1..
69 GO TO 60
70 10 CONTINUE
71 * CASE-SD1-NONNEGATIVE
72 SP2 = SD2*SY1
73 IF (.NOT.SP2.EQ.ZERO) GO TO 20
74 SFLAG = -TWO
75 GO TO 260
76 * REGULAR-CASE..
77 20 CONTINUE
78 SP1 = SD1*SX1
79 SQ2 = SP2*SY1
80 SQ1 = SP1*SX1
82 IF (.NOT.ABS(SQ1).GT.ABS(SQ2)) GO TO 40
83 SH21 = -SY1/SX1
84 SH12 = SP2/SP1
86 SU = ONE - SH12*SH21
88 IF (.NOT.SU.LE.ZERO) GO TO 30
89 * GO ZERO-H-D-AND-SX1..
90 GO TO 60
91 30 CONTINUE
92 SFLAG = ZERO
93 SD1 = SD1/SU
94 SD2 = SD2/SU
95 SX1 = SX1*SU
96 * GO SCALE-CHECK..
97 GO TO 100
98 40 CONTINUE
99 IF (.NOT.SQ2.LT.ZERO) GO TO 50
100 * GO ZERO-H-D-AND-SX1..
101 GO TO 60
102 50 CONTINUE
103 SFLAG = ONE
104 SH11 = SP1/SP2
105 SH22 = SX1/SY1
106 SU = ONE + SH11*SH22
107 STEMP = SD2/SU
108 SD2 = SD1/SU
109 SD1 = STEMP
110 SX1 = SY1*SU
111 * GO SCALE-CHECK
112 GO TO 100
113 * PROCEDURE..ZERO-H-D-AND-SX1..
114 60 CONTINUE
115 SFLAG = -ONE
116 SH11 = ZERO
117 SH12 = ZERO
118 SH21 = ZERO
119 SH22 = ZERO
121 SD1 = ZERO
122 SD2 = ZERO
123 SX1 = ZERO
124 * RETURN..
125 GO TO 220
126 * PROCEDURE..FIX-H..
127 70 CONTINUE
128 IF (.NOT.SFLAG.GE.ZERO) GO TO 90
130 IF (.NOT.SFLAG.EQ.ZERO) GO TO 80
131 SH11 = ONE
132 SH22 = ONE
133 SFLAG = -ONE
134 GO TO 90
135 80 CONTINUE
136 SH21 = -ONE
137 SH12 = ONE
138 SFLAG = -ONE
139 90 CONTINUE
140 GO TO IGO(120,150,180,210)
141 * PROCEDURE..SCALE-CHECK
142 100 CONTINUE
143 110 CONTINUE
144 IF (.NOT.SD1.LE.RGAMSQ) GO TO 130
145 IF (SD1.EQ.ZERO) GO TO 160
146 ASSIGN 120 TO IGO
147 * FIX-H..
148 GO TO 70
149 120 CONTINUE
150 SD1 = SD1*GAM**2
151 SX1 = SX1/GAM
152 SH11 = SH11/GAM
153 SH12 = SH12/GAM
154 GO TO 110
155 130 CONTINUE
156 140 CONTINUE
157 IF (.NOT.SD1.GE.GAMSQ) GO TO 160
158 ASSIGN 150 TO IGO
159 * FIX-H..
160 GO TO 70
161 150 CONTINUE
162 SD1 = SD1/GAM**2
163 SX1 = SX1*GAM
164 SH11 = SH11*GAM
165 SH12 = SH12*GAM
166 GO TO 140
167 160 CONTINUE
168 170 CONTINUE
169 IF (.NOT.ABS(SD2).LE.RGAMSQ) GO TO 190
170 IF (SD2.EQ.ZERO) GO TO 220
171 ASSIGN 180 TO IGO
172 * FIX-H..
173 GO TO 70
174 180 CONTINUE
175 SD2 = SD2*GAM**2
176 SH21 = SH21/GAM
177 SH22 = SH22/GAM
178 GO TO 170
179 190 CONTINUE
180 200 CONTINUE
181 IF (.NOT.ABS(SD2).GE.GAMSQ) GO TO 220
182 ASSIGN 210 TO IGO
183 * FIX-H..
184 GO TO 70
185 210 CONTINUE
186 SD2 = SD2/GAM**2
187 SH21 = SH21*GAM
188 SH22 = SH22*GAM
189 GO TO 200
190 220 CONTINUE
191 IF (SFLAG) 250,230,240
192 230 CONTINUE
193 SPARAM(3) = SH21
194 SPARAM(4) = SH12
195 GO TO 260
196 240 CONTINUE
197 SPARAM(2) = SH11
198 SPARAM(5) = SH22
199 GO TO 260
200 250 CONTINUE
201 SPARAM(2) = SH11
202 SPARAM(3) = SH21
203 SPARAM(4) = SH12
204 SPARAM(5) = SH22
205 260 CONTINUE
206 SPARAM(1) = SFLAG
207 RETURN