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
        GRN_ON;
120
 
121
        if(RC_Quality > 140)
122
        {
123
                if(ModelIsFlying < 256)
124
                {
125
                        StickYaw = 0;
126
                        if(ModelIsFlying == 250)
127
                        {
128
                                UpdateCompassCourse = 1;
129
                                ReadingIntegralGyroYaw = 0;
130
                                SetPointYaw = 0;
131
                        }
132
                }
133
                else MKFlags |= (MKFLAG_FLY); // set fly flag
134
 
135
......
136
 
137
                                                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
138
                                                {
139
// if roll stick is centered and nick stick is down
140
                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
141
                                                        {
142
                                                                CompassCalState = 1;
143
        BeepTime = 1000;
144
}
145
 
146
(R/C data):
147
 
148
                // mapping of yaw
149
                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
150
                // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
151
                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
152
                {
153
                        if (StickYaw > 2) StickYaw-= 2;
154
                        else if (StickYaw< -2) StickYaw += 2;
155
                        else StickYaw = 0;
156
                }
157
 
158
                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
159
                {
160
                         StickYaw += ExternControl.Yaw;
161
                // disable I part of gyro control feedback
162
                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor =  0;
163
 
164
 
165
 
166
 
167
        // MeasurementCounter is incremented in the isr of analog.c
168
        if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
169
        {
170
                        if(ParamSet.DriftComp)
171
                        {
172
                                if(YawGyroDrift >  BALANCE_NUMBER/2) AdBiasGyroYaw++;
173
                                if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
174
                        }
175
                        YawGyroDrift = 0;
176
 
177
 
178
//  Yawing
179
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
180
        if(abs(StickYaw) > 15 ) // yaw stick is activated
181
        {
182
                BadCompassHeading = 1000;
183
                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
184
                {
185
                        UpdateCompassCourse = 1;
186
                }
187
        }
188
        // exponential stick sensitivity in yawring rate
189
        tmp_int  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx?
190
        tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
191
 
192
        SetPointYaw = tmp_int;
193
        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
194
        ReadingIntegralGyroYaw -= tmp_int;
195
        // limit the effect
196
        CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
197
 
198
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
199
//  Compass
200
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
201
    // compass code is used if Compass option is selected
202
        if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
203
        {
204
                int16_t w, v, r,correction, error;
205
 
206
                if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
207
                {
208
                        SetCompassCalState();
209
                        #ifdef USE_KILLAGREG
210
                        MM3_Calibrate();
211
                        #endif
212
                }
213
                else
214
                {
215
                        #ifdef USE_KILLAGREG
216
                        static uint8_t updCompass = 0;
217
                        if (!updCompass--)
218
                        {
219
                                updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
220
                                MM3_Heading();
221
                        }
222
                        #endif
223
 
224
                        // get maximum attitude angle
225
                        w = abs(IntegralGyroNick / 512);
226
                        v = abs(IntegralGyroRoll / 512);
227
                        if(v > w) w = v;
228
                        correction = w / 8 + 1;
229
                        // calculate the deviation of the yaw gyro heading and the compass heading
230
                        if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
231
                        else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
232
                        if(abs(GyroYaw) > 128) // spinning fast
233
                        {
234
                                error = 0;
235
                        }
236
                        if(!BadCompassHeading && w < 25)
237
                        {
238
                                YawGyroDrift += error;
239
                                if(UpdateCompassCourse)
240
                                {
241
                                        BeepTime = 200;
242
                                        YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
243
                                        CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
244
                                        UpdateCompassCourse = 0;
245
                                }
246
                        }
247
                        YawGyroHeading += (error * 8) / correction;
248
                        w = (w * FCParam.CompassYawEffect) / 32;
249
                        w = FCParam.CompassYawEffect - w;
250
                        if(w >= 0)
251
                        {
252
                                if(!BadCompassHeading)
253
                                {
254
                                        v = 64 + (MaxStickNick + MaxStickRoll) / 8;
255
                                        // calc course deviation
256
                                        r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
257
                                        v = (r * w) / v; // align to compass course
258
                                        // limit yaw rate
259
                                        w = 3 * FCParam.CompassYawEffect;
260
                                        if (v > w) v = w;
261
                                        else if (v < -w) v = -w;
262
                                        ReadingIntegralGyroYaw += v;
263
                                }
264
                                else
265
                                { // wait a while
266
                                        BadCompassHeading--;
267
                                }
268
                        }
269
                        else
270
                        {  // ignore compass at extreme attitudes for a while
271
                                BadCompassHeading = 500;
272
                        }
273
                }
274
        }
275
 
276
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
277
        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING));
278
 
279
 
280
 
281
 
282
    YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;     // yaw controller
283
        #define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
284
        // limit YawMixFraction
285
        if(GasMixFraction > MIN_YAWGAS)
286
        {
287
                CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
288
        }
289
        else
290
        {
291
                CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
292
        }
293
        tmp_int = ParamSet.GasMax * CONTROL_SCALING;
294
        CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
295
 
296
 
297
 
298
 
299
 
300
 
301
 
302
 
303
---------------
304
 
305
 
306
 
307
anal-lyse:
308
 
309
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING;
310
OK
311
 
312
SetPointYaw->setPointYaw:
313
- static
314
- Set to 0 at take off
315
- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw)
316
OK
317
 
318
PDPartYaw:
319
- nonstatic nonglobal
320
- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor)
321
    /
322
    (256L / CONTROL_SCALING)
323
  + (int32_t)(IntegralGyroYaw * GyroYawIFactor)
324
    /
325
    (2 * (44000 / CONTROL_SCALING));
326
OK
327
 
328
GyroYaw:
329
- global
330
- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro)
331
OK
332
 
333
YawGyroHeading->yawGyroHeading:
334
- GyroYaw is summed to it at each iteration
335
- It is wrapped around at <0 and >=360.
336
- It is used -- where???? To adjust CompassCourse...
337
OK
338
 
339
IntegralGyroYaw->yawAngle: Deviation of heading from desired???
340
- GyroYaw is summed to it at each iteration
341
- SetPointYaw is subtracted from it (!) at each iteration.
342
- It is NOT wrapped, but just limited to +/- 50000
343
- It is corrected / pulled in axis coupling and by the compass.
344
OK (Except that with the compass)
345
 
346
BadCompassHeading: Need to update the "CompassCourse".
347
 
348
CompassHeading: Opdateret fra mm3mag.
349
 
350
CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag.
351
 
352
 
353
UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR