Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1775 - 1
// gyro readings
2
int16_t GyroNick, GyroRoll, GyroYaw;
3
int16_t TrimNick, TrimRoll;
4
 
5
 
6
int32_t IntegralGyroYaw = 0;
7
int32_t ReadingIntegralGyroYaw = 0;
8
 
9
// compass course
10
int16_t CompassHeading = -1; // negative angle indicates invalid data.
11
int16_t CompassCourse = -1;
12
int16_t CompassOffCourse = 0;
13
uint8_t CompassCalState = 0;
14
 
15
uint16_t BadCompassHeading = 500;
16
 
17
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
18
int16_t YawGyroDrift;
19
 
20
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
21
 
22
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
23
int16_t GPSStickNick = 0, GPSStickRoll = 0;
24
 
25
// stick values derived by uart inputs
26
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
27
 
28
/************************************************************************/
29
/*  Neutral Readings                                                    */
30
/************************************************************************/
31
void SetNeutral(uint8_t AccAdjustment)
32
{
33
    ...
34
    AdBiasGyroYaw     = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
35
 
36
    GyroYaw = 0;
37
 
38
    CompassCourse = CompassHeading;
39
    // Inititialize YawGyroIntegral value with current compass heading
40
    YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
41
    YawGyroDrift = 0;
42
 
43
 
44
// Something completely different: Here's why the turn-over's were vars.
45
        TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
46
        TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;
47
 
48
}
49
 
50
/************************************************************************/
51
/*  Averaging Measurement Readings                                      */
52
/************************************************************************/
53
void Mean(void)
54
{
55
 
56
        GyroYaw   = AdBiasGyroYaw - AdValueGyroYaw;
57
        // Yaw
58
        // calculate yaw gyro integral (~ to rotation angle)
59
        YawGyroHeading += GyroYaw;
60
        ReadingIntegralGyroYaw  += GyroYaw;
61
 
62
 
63
        // Coupling fraction
64
        if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
65
        {
66
....
67
                YawGyroHeading += tmp14;
68
                if(!FCParam.AxisCouplingYawCorrection)  ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw
69
            if(abs(GyroYaw > 64))
70
                {
71
                        if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
72
                }
73
 
74
        }
75
 
76
        // Yaw
77
    // limit YawGyroHeading proportional to 0? to 360?
78
    if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR))      YawGyroHeading -= 360L * GYRO_DEG_FACTOR;  // 360? Wrap
79
 
80
        if(YawGyroHeading < 0)                                          YawGyroHeading += 360L * GYRO_DEG_FACTOR;
81
 
82
    IntegralGyroYaw    = ReadingIntegralGyroYaw;
83
 
84
}
85
 
86
void SetCompassCalState(void)
87
{
88
        static uint8_t stick = 1;
89
 
90
    // if nick is centered or top set stick to zero
91
        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;
92
        // if nick is down trigger to next cal state
93
        if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
94
        {
95
                stick = 1;
96
                CompassCalState++;
97
                if(CompassCalState < 5) Beep(CompassCalState);
98
                else BeepTime = 1000;
99
        }
100
}
101
 
102
 
103
 
104
/************************************************************************/
105
/*  MotorControl                                                        */
106
/************************************************************************/
107
void MotorControl(void)
108
{
109
        int16_t h, tmp_int;
110
 
111
        // Mixer Fractions that are combined for Motor Control
112
        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
113
 
114
        int16_t PDPartYaw;
115
        static int32_t SetPointYaw = 0;
116
        static uint16_t UpdateCompassCourse = 0;
117
 
118
        Mean();
119
 
120
        if(RC_Quality > 140)
121
        {
122
                if(ModelIsFlying < 256)
123
                {
124
                        StickYaw = 0;
125
                        if(ModelIsFlying == 250)
126
                        {
127
                                UpdateCompassCourse = 1;
128
                                ReadingIntegralGyroYaw = 0;
129
                                SetPointYaw = 0;
130
                        }
131
                }
132
                else MKFlags |= (MKFLAG_FLY); // set fly flag
133
 
134
......
135
 
136
                                                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
137
                                                {
138
// if roll stick is centered and nick stick is down
139
                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
140
                                                        {
141
                                                                CompassCalState = 1;
142
        BeepTime = 1000;
143
}
144
 
145
(R/C data):
146
 
147
                // mapping of yaw
148
                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
149
                // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
150
                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
151
                {
152
                        if (StickYaw > 2) StickYaw-= 2;
153
                        else if (StickYaw< -2) StickYaw += 2;
154
                        else StickYaw = 0;
155
                }
156
 
157
                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
158
                {
159
                         StickYaw += ExternControl.Yaw;
160
                // disable I part of gyro control feedback
161
                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor =  0;
162
 
163
 
164
 
165
 
166
        // MeasurementCounter is incremented in the isr of analog.c
167
        if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
168
        {
169
                        if(ParamSet.DriftComp)
170
                        {
171
                                if(YawGyroDrift >  BALANCE_NUMBER/2) AdBiasGyroYaw++;
172
                                if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
173
                        }
174
                        YawGyroDrift = 0;
175
 
176
 
177
//  Yawing
178
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
179
        if(abs(StickYaw) > 15 ) // yaw stick is activated
180
        {
181
                BadCompassHeading = 1000;
182
                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
183
                {
184
                        UpdateCompassCourse = 1;
185
                }
186
        }
187
        // exponential stick sensitivity in yawring rate
188
        tmp_int  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx?
189
        tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
190
 
191
        SetPointYaw = tmp_int;
192
        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
193
        ReadingIntegralGyroYaw -= tmp_int;
194
        // limit the effect
195
        CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
196
 
197
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
198
//  Compass
199
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
200
    // compass code is used if Compass option is selected
201
        if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
202
        {
203
                int16_t w, v, r,correction, error;
204
 
205
                if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
206
                {
207
                        SetCompassCalState();
208
                        #ifdef USE_KILLAGREG
209
                        MM3_Calibrate();
210
                        #endif
211
                }
212
                else
213
                {
214
                        #ifdef USE_KILLAGREG
215
                        static uint8_t updCompass = 0;
216
                        if (!updCompass--)
217
                        {
218
                                updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
219
                                MM3_Heading();
220
                        }
221
                        #endif
222
 
223
                        // get maximum attitude angle
224
                        w = abs(IntegralGyroNick / 512);
225
                        v = abs(IntegralGyroRoll / 512);
226
                        if(v > w) w = v;
227
                        correction = w / 8 + 1;
228
                        // calculate the deviation of the yaw gyro heading and the compass heading
229
                        if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
230
                        else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
231
                        if(abs(GyroYaw) > 128) // spinning fast
232
                        {
233
                                error = 0;
234
                        }
235
                        if(!BadCompassHeading && w < 25)
236
                        {
237
                                YawGyroDrift += error;
238
                                if(UpdateCompassCourse)
239
                                {
240
                                        BeepTime = 200;
241
                                        YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
242
                                        CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
243
                                        UpdateCompassCourse = 0;
244
                                }
245
                        }
246
                        YawGyroHeading += (error * 8) / correction;
247
                        w = (w * FCParam.CompassYawEffect) / 32;
248
                        w = FCParam.CompassYawEffect - w;
249
                        if(w >= 0)
250
                        {
251
                                if(!BadCompassHeading)
252
                                {
253
                                        v = 64 + (MaxStickNick + MaxStickRoll) / 8;
254
                                        // calc course deviation
255
                                        r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
256
                                        v = (r * w) / v; // align to compass course
257
                                        // limit yaw rate
258
                                        w = 3 * FCParam.CompassYawEffect;
259
                                        if (v > w) v = w;
260
                                        else if (v < -w) v = -w;
261
                                        ReadingIntegralGyroYaw += v;
262
                                }
263
                                else
264
                                { // wait a while
265
                                        BadCompassHeading--;
266
                                }
267
                        }
268
                        else
269
                        {  // ignore compass at extreme attitudes for a while
270
                                BadCompassHeading = 500;
271
                        }
272
                }
273
        }
274
 
275
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
276
        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING));
277
 
278
 
279
 
280
 
281
    YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;     // yaw controller
282
        #define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
283
        // limit YawMixFraction
284
        if(GasMixFraction > MIN_YAWGAS)
285
        {
286
                CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
287
        }
288
        else
289
        {
290
                CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
291
        }
292
        tmp_int = ParamSet.GasMax * CONTROL_SCALING;
293
        CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
294
 
295
 
296
 
297
 
298
 
299
 
300
 
301
 
302
---------------
303
 
304
 
305
 
306
anal-lyse:
307
 
308
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;
309
OK
310
 
311
SetPointYaw->setPointYaw:
312
- static
313
- Set to 0 at take off
314
- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw)
315
OK
316
 
317
PDPartYaw:
318
- nonstatic nonglobal
319
- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor)
320
    /
321
    (256L / CONTROL_SCALING)
322
  + (int32_t)(IntegralGyroYaw * GyroYawIFactor)
323
    /
324
    (2 * (44000 / CONTROL_SCALING));
325
OK
326
 
327
GyroYaw:
328
- global
329
- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro)
330
OK
331
 
332
YawGyroHeading->yawGyroHeading:
333
- GyroYaw is summed to it at each iteration
334
- It is wrapped around at <0 and >=360.
335
- It is used -- where???? To adjust CompassCourse...
336
OK
337
 
338
IntegralGyroYaw->yawAngle: Deviation of heading from desired???
339
- GyroYaw is summed to it at each iteration
340
- SetPointYaw is subtracted from it (!) at each iteration.
341
- It is NOT wrapped, but just limited to +/- 50000
342
- It is corrected / pulled in axis coupling and by the compass.
343
OK (Except that with the compass)
344
 
345
BadCompassHeading: Need to update the "CompassCourse".
346
 
347
CompassHeading: Opdateret fra mm3mag.
348
 
349
CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag.
350
 
351
 
352
UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR