Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1227 killagreg 1
/*#######################################################################################
2
Flight Control
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 04.2007 Holger Buss
6
// + Nur für den privaten Gebrauch
7
// + www.MikroKopter.com
8
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
9
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
10
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
11
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
12
// + bzgl. der Nutzungsbedingungen aufzunehmen.
13
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
14
// + Verkauf von Luftbildaufnahmen, usw.
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
17
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
18
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
20
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
21
// + eindeutig als Ursprung verlinkt werden
22
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
24
// + Benutzung auf eigene Gefahr
25
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
28
// + mit unserer Zustimmung zulässig
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
33
// + this list of conditions and the following disclaimer.
34
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
35
// +     from this software without specific prior written permission.
36
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
37
// +     for non-commercial use (directly or indirectly)
38
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
39
// +     with our written permission
40
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
41
// +     clearly linked as origin
42
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
43
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
44
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
45
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
46
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
47
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
48
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
49
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
50
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
51
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52
// +  POSSIBILITY OF SUCH DAMAGE.
53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
54
#include <stdlib.h>
55
#include <avr/io.h>
56
 
57
#include "main.h"
58
#include "eeprom.h"
59
#include "timer0.h"
60
#include "analog.h"
61
#include "fc.h"
62
#include "uart0.h"
63
#include "rc.h"
64
#include "twimaster.h"
65
#include "timer2.h"
66
#ifdef USE_KILLAGREG
67
#include "mm3.h"
68
#include "gps.h"
69
#endif
70
#ifdef USE_MK3MAG
71
#include "mk3mag.h"
72
#include "gps.h"
73
#endif
74
#include "led.h"
75
#ifdef USE_NAVICTRL
76
#include "spi.h"
77
#endif
78
 
79
 
80
#define STICK_GAIN 4
81
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
82
 
83
// gyro readings
84
int16_t GyroNick, GyroRoll, GyroYaw;
85
 
86
// gyro bias
87
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0;
88
 
89
// accelerations
90
int16_t AccNick, AccRoll, AccTop;
91
 
92
// neutral acceleration readings
93
int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0;
94
volatile float AdBiasAccTop = 0;
95
// the additive gyro rate corrections according to the axis coupling
96
int16_t TrimNick, TrimRoll;
97
 
98
 
99
// attitude gyro integrals
100
int32_t IntegralGyroNick = 0,IntegralGyroNick2 = 0;
101
int32_t IntegralGyroRoll = 0,IntegralGyroRoll2 = 0;
102
int32_t IntegralGyroYaw = 0;
103
int32_t ReadingIntegralGyroNick = 0, ReadingIntegralGyroNick2 = 0;
104
int32_t ReadingIntegralGyroRoll = 0, ReadingIntegralGyroRoll2 = 0;
105
int32_t ReadingIntegralGyroYaw = 0;
106
int32_t MeanIntegralGyroNick;
107
int32_t MeanIntegralGyroRoll;
108
 
109
// attitude acceleration integrals
110
int32_t MeanAccNick = 0, MeanAccRoll = 0;
111
volatile int32_t ReadingIntegralTop = 0;
112
 
113
// compass course
114
int16_t CompassHeading = -1; // negative angle indicates invalid data.
115
int16_t CompassCourse = -1;
116
int16_t CompassOffCourse = 0;
117
uint8_t CompassCalState = 0;
118
uint8_t FunnelCourse = 0;
119
uint16_t BadCompassHeading = 500;
120
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
121
int16_t YawGyroDrift;
122
 
123
 
124
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
125
 
126
 
127
// MK flags
128
uint16_t ModelIsFlying = 0;
129
uint8_t  volatile MKFlags = 0;
130
 
131
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
132
 
133
uint8_t GyroPFactor, GyroIFactor;       // the PD factors for the attitude control
134
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
135
 
136
int16_t Ki = 10300 / 33;
137
 
138
int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
139
 
140
 
141
uint8_t RequiredMotors = 0;
142
 
143
 
144
// stick values derived by rc channels readings
145
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
146
int16_t GPSStickNick = 0, GPSStickRoll = 0;
147
 
148
int16_t MaxStickNick = 0, MaxStickRoll = 0;
149
 
150
// stick values derived by uart inputs
151
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
152
 
153
int16_t ReadingHeight = 0;
154
int16_t SetPointHeight = 0;
155
 
156
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0;
157
 
158
uint8_t LoopingNick = 0, LoopingRoll = 0;
159
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
160
 
161
 
162
fc_param_t FCParam = {48,251,16,58,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,70,90,65,64,100,0,0,0};
163
 
164
 
165
 
166
/************************************************************************/
167
/*  Filter for motor value smoothing                                    */
168
/************************************************************************/
169
int16_t MotorSmoothing(int16_t newvalue, int16_t oldvalue)
170
{
171
        int16_t motor;
172
        if(newvalue > oldvalue) motor = (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
173
        else                                    motor = newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
174
        return(motor);
175
}
176
 
177
/************************************************************************/
178
/*  Creates numbeeps beeps at the speaker                               */
179
/************************************************************************/
180
void Beep(uint8_t numbeeps)
181
{
182
        while(numbeeps--)
183
        {
184
                if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
185
                BeepTime = 100; // 0.1 second
186
                Delay_ms(250); // blocks 250 ms as pause to next beep,
187
                // this will block the flight control loop,
188
                // therefore do not use this function if motors are running
189
        }
190
}
191
 
192
/************************************************************************/
193
/*  Neutral Readings                                                    */
194
/************************************************************************/
195
void SetNeutral(uint8_t AccAdjustment)
196
{
197
        uint8_t i;
198
        int32_t Sum_1, Sum_2 = 0, Sum_3;
199
 
200
        Servo_Off(); // disable servo output
201
 
202
        AdBiasAccNick = 0;
203
        AdBiasAccRoll = 0;
204
        AdBiasAccTop = 0;
205
 
206
    BiasHiResGyroNick = 0;
207
        BiasHiResGyroRoll = 0;
208
        AdBiasGyroYaw = 0;
209
 
210
    FCParam.AxisCoupling1 = 0;
211
    FCParam.AxisCoupling2 = 0;
212
 
213
    ExpandBaro = 0;
214
 
215
        // sample values with bias set to zero
216
    Delay_ms_Mess(100);
217
 
218
    if(BoardRelease == 13) SearchDacGyroOffset();
219
 
220
    if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Height Control activated?
221
    {
222
                if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
223
    }
224
 
225
    // determine gyro bias by averaging (require no rotation movement)
226
    #define GYRO_BIAS_AVERAGE 32
227
    Sum_1 = 0;
228
        Sum_2 = 0;
229
        Sum_3 = 0;
230
    for(i=0; i < GYRO_BIAS_AVERAGE; i++)
231
    {
232
                Delay_ms_Mess(10);
233
                Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY;
234
                Sum_2 += AdValueGyroRoll * HIRES_GYRO_AMPLIFY;
235
                Sum_3 += AdValueGyroYaw;
236
        }
237
        BiasHiResGyroNick = (int16_t)((Sum_1 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
238
        BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
239
        AdBiasGyroYaw     = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
240
 
241
    if(AccAdjustment)
242
    {
243
                // determine acc bias by averaging (require horizontal adjustment in nick and roll attitude)
244
                #define ACC_BIAS_AVERAGE 10
245
                Sum_1 = 0;
246
                Sum_2 = 0;
247
                Sum_3 = 0;
248
                for(i=0; i < ACC_BIAS_AVERAGE; i++)
249
                {
250
                        Delay_ms_Mess(10);
251
                        Sum_1 += AdValueAccNick;
252
                        Sum_2 += AdValueAccRoll;
253
                        Sum_3 += AdValueAccZ;
254
                }
255
                // use abs() to avoid negative bias settings because of adc sign flip in adc.c
256
                AdBiasAccNick = (int16_t)((abs(Sum_1) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);
257
                AdBiasAccRoll = (int16_t)((abs(Sum_2) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);
258
                AdBiasAccTop  = (int16_t)((abs(Sum_3) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE);
259
 
260
                // Save ACC neutral settings to eeprom
261
                SetParamWord(PID_ACC_NICK, (uint16_t)AdBiasAccNick);
262
                SetParamWord(PID_ACC_ROLL, (uint16_t)AdBiasAccRoll);
263
                SetParamWord(PID_ACC_TOP,  (uint16_t)AdBiasAccTop);
264
    }
265
    else // restore from eeprom
266
    {
267
                AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK);
268
            AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
269
            AdBiasAccTop  = (int16_t)GetParamWord(PID_ACC_TOP);
270
    }
271
    // setting acc bias values has an influence in the analog.c ISR
272
    // therefore run measurement for 100ms to achive stable readings
273
        Delay_ms_Mess(100);
274
 
275
    // reset acc averaging and integrals
276
    AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
277
    AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
278
    AccTop  = AdValueAccTop;
279
    ReadingIntegralTop = AdValueAccTop;
280
 
281
        // and gyro readings
282
        GyroNick = 0;
283
        GyroRoll = 0;
284
    GyroYaw = 0;
285
 
286
    // reset gyro integrals to acc guessing
287
    IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
288
    IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
289
        //ReadingIntegralGyroNick = IntegralGyroNick;
290
        //ReadingIntegralGyroRoll = IntegralGyroRoll;
291
    ReadingIntegralGyroNick2 = IntegralGyroNick;
292
    ReadingIntegralGyroRoll2 = IntegralGyroRoll;
293
    ReadingIntegralGyroYaw = 0;
294
 
295
 
296
    StartAirPressure = AirPressure;
297
    HeightD = 0;
298
 
299
        // update compass course to current heading
300
    CompassCourse = CompassHeading;
301
    // Inititialize YawGyroIntegral value with current compass heading
302
    YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
303
    YawGyroDrift = 0;
304
 
305
    BeepTime = 50;
306
 
307
        TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L;
308
        TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;
309
 
310
    ExternHeightValue = 0;
311
 
312
    GPSStickNick = 0;
313
    GPSStickRoll = 0;
314
 
315
    MKFlags |= MKFLAG_CALIBRATE;
316
 
317
        FCParam.KalmanK = -1;
318
        FCParam.KalmanMaxDrift = 0;
319
        FCParam.KalmanMaxFusion = 32;
320
 
321
        Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110;
322
        Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110;
323
        Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110;
324
        Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110;
325
 
326
        Servo_On(); //enable servo output
327
        RC_Quality = 100;
328
}
329
 
330
/************************************************************************/
331
/*  Averaging Measurement Readings                                      */
332
/************************************************************************/
333
void Mean(void)
334
{
335
    int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
336
    int16_t FilterGyroNick, FilterGyroRoll;
337
        static int16_t Last_GyroRoll = 0, Last_GyroNick = 0;
338
        int16_t d2Nick, d2Roll;
339
        int32_t AngleNick, AngleRoll;
340
        int16_t CouplingNickRoll = 0, CouplingRollNick = 0;
341
 
342
        // Get bias free gyro readings
343
        GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
344
    FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
345
 
346
        // handle rotation rates that violate adc ranges
347
        if(AdValueGyroNick < 15)   GyroNick = -1000;
348
        if(AdValueGyroNick <  7)   GyroNick = -2000;
349
        if(BoardRelease == 10)
350
        {
351
                if(AdValueGyroNick > 1010) GyroNick = +1000;
352
                if(AdValueGyroNick > 1017) GyroNick = +2000;
353
        }
354
        else
355
        {
356
                if(AdValueGyroNick > 2000) GyroNick = +1000;
357
                if(AdValueGyroNick > 2015) GyroNick = +2000;
358
        }
359
 
360
        GyroRoll = HiResGyroRoll / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
361
        FilterGyroRoll = FilterHiResGyroRoll / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
362
        // handle rotation rates that violate adc ranges
363
        if(AdValueGyroRoll < 15)   GyroRoll = -1000;
364
        if(AdValueGyroRoll <  7)   GyroRoll = -2000;
365
        if(BoardRelease == 10)
366
        {
367
                if(AdValueGyroRoll > 1010) GyroRoll = +1000;
368
                if(AdValueGyroRoll > 1017) GyroRoll = +2000;
369
        }
370
        else
371
        {
372
                if(AdValueGyroRoll > 2000) GyroRoll = +1000;
373
                if(AdValueGyroRoll > 2015) GyroRoll = +2000;
374
        }
375
 
376
        GyroYaw   = AdBiasGyroYaw - AdValueGyroYaw;
377
 
378
        // Acceleration Sensor
379
        // lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution
380
        AccNick  = ((int32_t)AccNick * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
381
        AccRoll  = ((int32_t)AccRoll * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
382
        AccTop   = ((int32_t)AccTop  * 3 + ((int32_t)AdValueAccTop)) / 4L;
383
 
384
        // sum acc sensor readings for later averaging
385
    MeanAccNick  += ACC_AMPLIFY * AdValueAccNick;
386
    MeanAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
387
 
388
    NaviAccNick += AdValueAccNick;
389
    NaviAccRoll += AdValueAccRoll;
390
    NaviCntAcc++;
391
 
392
 
393
        // enable ADC to meassure next readings, before that point all variables should be read that are written by the ADC ISR
394
        ADC_Enable();
395
        ADReady = 0;
396
 
397
        // limit angle readings for axis coupling calculations
398
        #define ANGLE_LIMIT 93000L // aprox. 93000/GYRO_DEG_FACTOR = 82 deg
399
 
400
        AngleNick = ReadingIntegralGyroNick;
401
        CHECK_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT);
402
 
403
        AngleRoll = ReadingIntegralGyroRoll;
404
        CHECK_MIN_MAX(AngleRoll, -ANGLE_LIMIT, ANGLE_LIMIT);
405
 
406
 
407
        // Yaw
408
        // calculate yaw gyro integral (~ to rotation angle)
409
        YawGyroHeading += GyroYaw;
410
        ReadingIntegralGyroYaw  += GyroYaw;
411
 
412
 
413
        // Coupling fraction
414
        if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
415
        {
416
                tmp13 = (FilterGyroRoll * AngleNick) / 2048L;
417
                tmp13 *= FCParam.AxisCoupling2; // 65
418
                tmp13 /= 4096L;
419
                CouplingNickRoll = tmp13;
420
 
421
                tmp14 = (FilterGyroNick * AngleRoll) / 2048L;
422
                tmp14 *= FCParam.AxisCoupling2; // 65
423
                tmp14 /= 4096L;
424
                CouplingRollNick = tmp14;
425
 
426
                tmp14 -= tmp13;
427
                YawGyroHeading += tmp14;
428
                if(!FCParam.AxisCouplingYawCorrection)  ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw
429
 
430
                tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L;
431
                tmpl *= FCParam.AxisCoupling1;
432
                tmpl /= 4096L;
433
 
434
                tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L;
435
                tmpl2 *= FCParam.AxisCoupling1;
436
                tmpl2 /= 4096L;
437
                if(abs(GyroYaw > 64))
438
                {
439
                        if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
440
                }
441
 
442
                TrimNick = -tmpl2 + tmpl / 100L;
443
                TrimRoll = tmpl - tmpl2 / 100L;
444
        }
445
        else
446
        {
447
                CouplingNickRoll = 0;
448
                CouplingRollNick = 0;
449
                TrimNick = 0;
450
                TrimRoll = 0;
451
        }
452
 
453
 
454
        // Yaw
455
 
456
    // limit YawGyroHeading proportional to 0° to 360°
457
    if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR))      YawGyroHeading -= 360L * GYRO_DEG_FACTOR;  // 360° Wrap
458
        if(YawGyroHeading < 0)                                          YawGyroHeading += 360L * GYRO_DEG_FACTOR;
459
 
460
        // Roll
461
        ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll;
462
        ReadingIntegralGyroRoll  += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll;
463
        if(ReadingIntegralGyroRoll > TurnOver180Roll)
464
        {
465
                ReadingIntegralGyroRoll  = -(TurnOver180Roll - 10000L);
466
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
467
        }
468
        if(ReadingIntegralGyroRoll < -TurnOver180Roll)
469
        {
470
                ReadingIntegralGyroRoll =  (TurnOver180Roll - 10000L);
471
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
472
        }
473
 
474
        // Nick
475
        ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick;
476
        ReadingIntegralGyroNick  += FilterGyroNick + TrimNick - AttitudeCorrectionNick;
477
        if(ReadingIntegralGyroNick > TurnOver180Nick)
478
        {
479
                ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L);
480
                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
481
        }
482
        if(ReadingIntegralGyroNick < -TurnOver180Nick)
483
        {
484
                ReadingIntegralGyroNick =  (TurnOver180Nick - 25000L);
485
                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
486
        }
487
 
488
    IntegralGyroYaw    = ReadingIntegralGyroYaw;
489
    IntegralGyroNick   = ReadingIntegralGyroNick;
490
    IntegralGyroRoll   = ReadingIntegralGyroRoll;
491
    IntegralGyroNick2  = ReadingIntegralGyroNick2;
492
    IntegralGyroRoll2  = ReadingIntegralGyroRoll2;
493
 
494
 
495
        #define D_LIMIT 128
496
 
497
        if(FCParam.GyroD)
498
        {
499
                d2Nick = (HiResGyroNick - Last_GyroNick); // change of gyro rate
500
                Last_GyroNick = (Last_GyroNick + HiResGyroNick) / 2;
501
                CHECK_MIN_MAX(d2Nick, -D_LIMIT, D_LIMIT);
502
                GyroNick += (d2Nick * (int16_t)FCParam.GyroD) / 16;
503
 
504
                d2Roll = (HiResGyroRoll - Last_GyroRoll); // change of gyro rate
505
                Last_GyroRoll = (Last_GyroRoll + HiResGyroRoll) / 2;
506
                CHECK_MIN_MAX(d2Roll, -D_LIMIT, D_LIMIT);
507
                GyroRoll += (d2Roll * (int16_t)FCParam.GyroD) / 16;
508
 
509
                HiResGyroNick += (d2Nick * (int16_t)FCParam.GyroD);
510
                HiResGyroRoll += (d2Roll * (int16_t)FCParam.GyroD);
511
        }
512
 
513
        // Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation
514
        if(FilterGyroNick > 0)  TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
515
        else                    TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L;
516
        if(FilterGyroRoll > 0)  TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
517
        else                    TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L;
518
 
519
        // increase the nick/roll rates virtually from the threshold of 245 to slow down higher rotation rates
520
        if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && ! LoopingNick && !LoopingRoll)
521
        {
522
                if(FilterGyroNick > 256)                GyroNick += 1 * (FilterGyroNick - 256);
523
                else if(FilterGyroNick < -256)  GyroNick += 1 * (FilterGyroNick + 256);
524
                if(FilterGyroRoll > 256)        GyroRoll += 1 * (FilterGyroRoll - 256);
525
                else if(FilterGyroRoll < -256)  GyroRoll += 1 * (FilterGyroRoll + 256);
526
        }
527
 
528
}
529
 
530
 
531
/************************************************************************/
532
/*  Transmit Motor Data via I2C                                         */
533
/************************************************************************/
534
void SendMotorData(void)
535
{
536
        uint8_t i;
537
    if(!(MKFlags & MKFLAG_MOTOR_RUN))
538
    {
539
                MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
540
                for(i = 0; i < MAX_MOTORS; i++)
541
                {
542
                        if(!MotorTest_Active) Motor[i].SetPoint = 0;
543
                        else                  Motor[i].SetPoint = MotorTest[i];
544
                }
545
                if(MotorTest_Active) MotorTest_Active--;
546
        }
547
 
548
        DebugOut.Analog[12] = Motor[0].SetPoint; // Front
549
        DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
550
        DebugOut.Analog[14] = Motor[3].SetPoint; // Left
551
        DebugOut.Analog[15] = Motor[2].SetPoint; // Right
552
    //Start I2C Interrupt Mode
553
    I2C_Start(TWI_STATE_MOTOR_TX);
554
}
555
 
556
 
557
/************************************************************************/
558
/*  Map the parameter to poti values                                    */
559
/************************************************************************/
560
void ParameterMapping(void)
561
{
562
        if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok
563
        // else the last updated values are used
564
        {
565
                 //update poti values by rc-signals
566
                #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
567
                #define CHK_POTI(b,a) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a;}
568
                CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight);
569
                CHK_POTI_MM(FCParam.HeightD,ParamSet.HeightD,0,100);
570
                CHK_POTI_MM(FCParam.HeightP,ParamSet.HeightP,0,100);
571
                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
572
                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
573
                CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255);
574
                CHK_POTI(FCParam.GyroI,ParamSet.GyroI);
575
                CHK_POTI(FCParam.GyroD,ParamSet.GyroD);
576
                CHK_POTI(FCParam.IFactor,ParamSet.IFactor);
577
                CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1);
578
                CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2);
579
                CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3);
580
                CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4);
581
                CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5);
582
                CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6);
583
                CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7);
584
                CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8);
585
                CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl);
586
                CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit);
587
                CHK_POTI(FCParam.AxisCoupling1,ParamSet.AxisCoupling1);
588
                CHK_POTI(FCParam.AxisCoupling2,ParamSet.AxisCoupling2);
589
                CHK_POTI(FCParam.AxisCouplingYawCorrection,ParamSet.AxisCouplingYawCorrection);
590
                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
591
                CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
592
                CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
593
                #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
594
                CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl);
595
                CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain);
596
                CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP);
597
                CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
598
                CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
599
                CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
600
                CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);
601
                CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);
602
                CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
603
                #endif
604
                CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
605
                Ki = 10300 / ( FCParam.IFactor + 1 );
606
        }
607
}
608
 
609
 
610
void SetCompassCalState(void)
611
{
612
        static uint8_t stick = 1;
613
 
614
    // if nick is centered or top set stick to zero
615
        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0;
616
        // if nick is down trigger to next cal state
617
        if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
618
        {
619
                stick = 1;
620
                CompassCalState++;
621
                if(CompassCalState < 5) Beep(CompassCalState);
622
                else BeepTime = 1000;
623
        }
624
}
625
 
626
 
627
 
628
/************************************************************************/
629
/*  MotorControl                                                        */
630
/************************************************************************/
631
void MotorControl(void)
632
{
633
        int16_t h, tmp_int;
634
 
635
        // Mixer Fractions that are combined for Motor Control
636
        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
637
 
638
        // PID controller variables
639
        int16_t DiffNick, DiffRoll;
640
        int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll;
641
        static int32_t IPartNick = 0, IPartRoll = 0;
642
 
643
        static int32_t SetPointYaw = 0;
644
        static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0;
645
        static int32_t CorrectionNick, CorrectionRoll;
646
        static uint16_t RcLostTimer;
647
        static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
648
        static uint8_t HeightControlActive = 0;
649
        static int16_t HeightControlGas = 0;
650
        static int8_t  TimerDebugOut = 0;
651
        static uint16_t UpdateCompassCourse = 0;
652
        // high resolution motor values for smoothing of PID motor outputs
653
        static int16_t MotorValue[MAX_MOTORS];
654
        uint8_t i;
655
 
656
        Mean();
657
        GRN_ON;
658
 
659
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
660
// determine gas value
661
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
662
        GasMixFraction = StickGas;
663
    if(GasMixFraction < ParamSet.GasMin + 10) GasMixFraction = ParamSet.GasMin + 10;
664
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
665
// RC-signal is bad
666
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
667
        if(RC_Quality < 120)  // the rc-frame signal is not reveived or noisy
668
        {
669
                if(!PcAccess) // if also no PC-Access via UART
670
                {
671
                        if(BeepModulation == 0xFFFF)
672
                        {
673
                                BeepTime = 15000; // 1.5 seconds
674
                                BeepModulation = 0x0C00;
675
                        }
676
                }
677
                if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost
678
                else // rc lost countdown finished
679
                {
680
                        MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
681
                }
682
                RED_ON; // set red led
683
                if(ModelIsFlying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
684
                {
685
                        GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
686
                        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing
687
                        // set neutral rc inputs
688
                        PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
689
                        PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
690
                        PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
691
                        PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
692
                        PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
693
                        PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
694
                }
695
                else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData()
696
        } // eof RC_Quality < 120
697
        else
698
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
699
// RC-signal is good
700
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
701
        if(RC_Quality > 140)
702
        {
703
                MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
704
                // reset emergency timer
705
                RcLostTimer = ParamSet.EmergencyGasDuration * 50;
706
                if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
707
                {
708
                        if(ModelIsFlying < 0xFFFF) ModelIsFlying++;
709
                }
710
                if(ModelIsFlying < 256)
711
                {
712
                        IPartNick = 0;
713
                        IPartRoll = 0;
714
                        StickYaw = 0;
715
                        if(ModelIsFlying == 250)
716
                        {
717
                                UpdateCompassCourse = 1;
718
                                ReadingIntegralGyroYaw = 0;
719
                                SetPointYaw = 0;
720
                        }
721
                }
722
                else MKFlags |= (MKFLAG_FLY); // set fly flag
723
 
724
                if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
725
                if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
726
                if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
727
                if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
728
                //PPM24-Extension
729
                if(Poti5 < PPM_in[9] + 110)  Poti5++; else if(Poti5 >  PPM_in[9] + 110 && Poti5) Poti5--;
730
                if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
731
                if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
732
                if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
733
                //limit poti values
734
                if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
735
                if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
736
                if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
737
                if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
738
                //PPM24-Extension
739
                if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
740
                if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
741
                if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
742
                if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
743
 
744
                // if motors are off and the gas stick is in the upper position
745
                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
746
                {
747
                        // and if the yaw stick is in the leftmost position
748
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
749
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
750
// calibrate the neutral readings of all attitude sensors
751
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
752
                        {
753
                                // gas/yaw joystick is top left
754
                                //  _________
755
                                // |x        |
756
                                // |         |
757
                                // |         |
758
                                // |         |
759
                                // |         |
760
                                //  ¯¯¯¯¯¯¯¯¯
761
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
762
                                {
763
                                        delay_neutral = 0;
764
                                        GRN_OFF;
765
                                        ModelIsFlying = 0;
766
                                        // check roll/nick stick position
767
                                        // if nick stick is top or roll stick is left or right --> change parameter setting
768
                                        // according to roll/nick stick position
769
                                        if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
770
                                        {
771
                                                 uint8_t setting = 1; // default
772
                                                 // nick/roll joystick
773
                                                 //  _________
774
                                                 // |2   3   4|
775
                                                 // |         |
776
                                                 // |1       5|
777
                                                 // |         |
778
                                                 // |         |
779
                                                 //  ¯¯¯¯¯¯¯¯¯
780
                                                 // roll stick leftmost and nick stick centered --> setting 1
781
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1;
782
                                                 // roll stick leftmost and nick stick topmost --> setting 2
783
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2;
784
                                                 // roll stick centered an nick stick topmost --> setting 3
785
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3;
786
                                                 // roll stick rightmost and nick stick topmost --> setting 4
787
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4;
788
                                                 // roll stick rightmost and nick stick centered --> setting 5
789
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5;
790
                                                 // update active parameter set in eeprom
791
                                                 SetActiveParamSet(setting);
792
                                                 ParamSet_ReadFromEEProm(GetActiveParamSet());
793
                                                 SetNeutral(NO_ACC_CALIB);
794
                                                 Beep(GetActiveParamSet());
795
                                        }
796
                                        else
797
                                        {
798
                                                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
799
                                                {
800
                                                        // if roll stick is centered and nick stick is down
801
                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
802
                                                        {
803
                                                                // nick/roll joystick
804
                                                                //  _________
805
                                                                // |         |
806
                                                                // |         |
807
                                                                // |         |
808
                                                                // |         |
809
                                                                // |    x    |
810
                                                                //  ¯¯¯¯¯¯¯¯¯
811
                                                                // enable calibration state of compass
812
                                                                CompassCalState = 1;
813
                                                                BeepTime = 1000;
814
                                                        }
815
                                                        else // nick and roll are centered
816
                                                        {
817
                                                                ParamSet_ReadFromEEProm(GetActiveParamSet());
818
                                                                SetNeutral(NO_ACC_CALIB);
819
                                                                Beep(GetActiveParamSet());
820
                                                        }
821
                                                }
822
                                                else // nick and roll are centered
823
                                                {
824
                                                        ParamSet_ReadFromEEProm(GetActiveParamSet());
825
                                                        SetNeutral(NO_ACC_CALIB);
826
                                                        Beep(GetActiveParamSet());
827
                                                }
828
                                        }
829
                                }
830
                        }
831
                        // and if the yaw stick is in the rightmost position
832
                        // save the ACC neutral setting to eeprom
833
                        else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
834
                        {
835
                                // gas/yaw joystick is top right
836
                                //  _________
837
                                // |        x|
838
                                // |         |
839
                                // |         |
840
                                // |         |
841
                                // |         |
842
                                //  ¯¯¯¯¯¯¯¯¯
843
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
844
                                {
845
                                        delay_neutral = 0;
846
                                        GRN_OFF;
847
                                        ModelIsFlying = 0;
848
                                        SetNeutral(ACC_CALIB);
849
                                        Beep(GetActiveParamSet());
850
                                }
851
                        }
852
                        else delay_neutral = 0;
853
                }
854
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
855
// gas stick is down
856
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
857
                if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
858
                {
859
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
860
                        {
861
                                // gas/yaw joystick is bottom right
862
                                //  _________
863
                                // |         |
864
                                // |         |
865
                                // |         |
866
                                // |         |
867
                                // |        x|
868
                                //  ¯¯¯¯¯¯¯¯¯
869
                                // Start Motors
870
                                if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
871
                                {
872
                                        delay_startmotors = 200; // do not repeat if once executed
873
                                        ModelIsFlying = 1;
874
                                        MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
875
                                        SetPointYaw = 0;
876
                                        ReadingIntegralGyroYaw = 0;
877
                                        ReadingIntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
878
                                        ReadingIntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
879
                                        ReadingIntegralGyroNick2 = IntegralGyroNick;
880
                                        ReadingIntegralGyroRoll2 = IntegralGyroRoll;
881
                                        IPartNick = 0;
882
                                        IPartRoll = 0;
883
                                }
884
                        }
885
                        else delay_startmotors = 0; // reset delay timer if sticks are not in this position
886
 
887
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
888
                        {
889
                                // gas/yaw joystick is bottom left
890
                                //  _________
891
                                // |         |
892
                                // |         |
893
                                // |         |
894
                                // |         |
895
                                // |x        |
896
                                //  ¯¯¯¯¯¯¯¯¯
897
                                // Stop Motors
898
                                if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
899
                                {
900
                                        delay_stopmotors = 200; // do not repeat if once executed
901
                                        ModelIsFlying = 0;
902
                                        MKFlags &= ~(MKFLAG_MOTOR_RUN);
903
                                }
904
                        }
905
                        else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
906
                }
907
                        // remapping of paameters only if the signal rc-sigbnal conditions are good
908
        } // eof RC_Quality > 150
909
 
910
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
911
// new values from RC
912
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
913
        if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC
914
        {
1240 killagreg 915
                static int16_t stick_nick = 0, stick_roll = 0;
916
 
1227 killagreg 917
                ParameterMapping(); // remapping params (online poti replacement)
1240 killagreg 918
 
1227 killagreg 919
                // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
1240 killagreg 920
                stick_nick = (stick_nick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4;
921
                stick_nick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD;
922
                StickNick = stick_nick - GPSStickNick;
1227 killagreg 923
 
1240 killagreg 924
                stick_roll = (stick_roll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4;
925
                stick_roll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD;
926
                StickRoll = stick_roll - GPSStickRoll;
1227 killagreg 927
 
928
                // mapping of yaw
929
                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
930
                // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
931
                if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
932
                {
933
                        if (StickYaw > 2) StickYaw-= 2;
934
                        else if (StickYaw< -2) StickYaw += 2;
935
                        else StickYaw = 0;
936
                }
937
 
938
                // mapping of gas
939
                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
940
 
941
                // update gyro control loop factors
942
                GyroPFactor = FCParam.GyroP + 10;
943
                GyroIFactor = FCParam.GyroI;
944
                GyroYawPFactor = FCParam.GyroP + 10;
945
                GyroYawIFactor = FCParam.GyroI;
946
 
947
 
948
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
949
//+ Analog control via serial communication
950
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
951
 
952
                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
953
                {
954
                         StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.StickP;
955
                         StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.StickP;
956
                         StickYaw += ExternControl.Yaw;
957
                         ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
958
                         if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
959
                }
960
                if(StickGas < 0) StickGas = 0;
961
 
962
                // disable I part of gyro control feedback
963
                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor =  0;
964
 
965
                // update max stick positions for nick and roll
966
                if(abs(StickNick / STICK_GAIN) > MaxStickNick)
967
                {
968
                        MaxStickNick = abs(StickNick)/STICK_GAIN;
969
                        if(MaxStickNick > 100) MaxStickNick = 100;
970
                }
971
                else MaxStickNick--;
972
                if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
973
                {
974
                        MaxStickRoll = abs(StickRoll)/STICK_GAIN;
975
                        if(MaxStickRoll > 100) MaxStickRoll = 100;
976
                }
977
                else MaxStickRoll--;
978
 
979
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
980
// Looping?
981
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
982
 
983
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT)  LoopingLeft = 1;
984
                else
985
                {
986
                        if(LoopingLeft) // Hysteresis
987
                        {
988
                                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingLeft = 0;
989
                        }
990
                }
991
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) LoopingRight = 1;
992
                else
993
                {
994
                        if(LoopingRight) // Hysteresis
995
                        {
996
                                if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingRight = 0;
997
                        }
998
                }
999
 
1000
                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_UP) LoopingTop = 1;
1001
                else
1002
                {
1003
                        if(LoopingTop)  // Hysteresis
1004
                        {
1005
                                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingTop = 0;
1006
                        }
1007
                }
1008
                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_DOWN) LoopingDown = 1;
1009
                else
1010
                {
1011
                        if(LoopingDown) // Hysteresis
1012
                        {
1013
                                if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0;
1014
                        }
1015
                }
1016
 
1017
                if(LoopingLeft || LoopingRight)  LoopingRoll = 1; else LoopingRoll = 0;
1018
                if(LoopingTop  || LoopingDown) { LoopingNick = 1; LoopingRoll = 0; LoopingLeft = 0; LoopingRight = 0;} else  LoopingNick = 0;
1019
        } // End of new RC-Values or Emergency Landing
1020
 
1021
 
1022
        if(LoopingRoll ||  LoopingNick)
1023
        {
1024
                if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit;
1025
                FunnelCourse = 1;
1026
        }
1027
 
1028
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1029
// in case of emergency landing
1030
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1031
        // set all inputs to save values
1032
        if(MKFlags & MKFLAG_EMERGENCY_LANDING)
1033
        {
1034
                StickYaw = 0;
1035
                StickNick = 0;
1036
                StickRoll = 0;
1037
                GyroPFactor  = 90;
1038
                GyroIFactor  = 120;
1039
                GyroYawPFactor = 90;
1040
                GyroYawIFactor = 120;
1041
                LoopingRoll = 0;
1042
                LoopingNick = 0;
1043
                MaxStickNick = 0;
1044
                MaxStickRoll = 0;
1045
        }
1046
 
1047
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1048
// Trim Gyro-Integrals to ACC-Signals
1049
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1050
 
1051
        #define BALANCE_NUMBER 256L
1052
        // sum for averaging
1053
        MeanIntegralGyroNick  += IntegralGyroNick;
1054
        MeanIntegralGyroRoll  += IntegralGyroRoll;
1055
 
1056
        if( LoopingNick || LoopingRoll) // if looping in any direction
1057
        {
1058
                // reset averaging for acc and gyro integral as well as gyro integral acc correction
1059
                MeasurementCounter = 0;
1060
 
1061
                MeanAccNick = 0;
1062
                MeanAccRoll = 0;
1063
 
1064
                MeanIntegralGyroNick = 0;
1065
                MeanIntegralGyroRoll = 0;
1066
 
1067
                ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
1068
                ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
1069
 
1070
                AttitudeCorrectionNick = 0;
1071
                AttitudeCorrectionRoll = 0;
1072
        }
1073
 
1074
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1075
        if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (MKFlags & MKFLAG_MOTOR_RUN)  ) ) // if not lopping in any direction
1076
        {
1077
                int32_t tmp_long, tmp_long2;
1078
                if( FCParam.KalmanK != -1)
1079
                {
1080
                        // determine the deviation of gyro integral from averaged acceleration sensor
1081
                        tmp_long   = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick);
1082
                        tmp_long   = (tmp_long  * FCParam.KalmanK) / (32 * 16);
1083
                        tmp_long2  = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
1084
                        tmp_long2  = (tmp_long2 * FCParam.KalmanK) / (32 * 16);
1085
 
1086
                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
1087
                        {
1088
                                tmp_long  /= 2;
1089
                                tmp_long2 /= 2;
1090
                        }
1091
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
1092
                        {
1093
                                tmp_long  /= 3;
1094
                                tmp_long2 /= 3;
1095
                        }
1096
                        // limit correction effect
1097
                        if(tmp_long >  (int32_t)FCParam.KalmanMaxFusion)  tmp_long  = (int32_t)FCParam.KalmanMaxFusion;
1098
                        if(tmp_long < -(int32_t)FCParam.KalmanMaxFusion)  tmp_long  =-(int32_t)FCParam.KalmanMaxFusion;
1099
                        if(tmp_long2 > (int32_t)FCParam.KalmanMaxFusion)  tmp_long2 = (int32_t)FCParam.KalmanMaxFusion;
1100
                        if(tmp_long2 <-(int32_t)FCParam.KalmanMaxFusion)  tmp_long2 =-(int32_t)FCParam.KalmanMaxFusion;
1101
                }
1102
                else
1103
                {
1104
                        // determine the deviation of gyro integral from acceleration sensor
1105
                        tmp_long   = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick);
1106
                        tmp_long  /= 16;
1107
                        tmp_long2  = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
1108
                        tmp_long2 /= 16;
1109
 
1110
                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
1111
                        {
1112
                                tmp_long  /= 3;
1113
                                tmp_long2 /= 3;
1114
                        }
1115
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
1116
                        {
1117
                                tmp_long  /= 3;
1118
                                tmp_long2 /= 3;
1119
                        }
1120
 
1121
                        #define BALANCE 32
1122
                        // limit correction effect
1123
                        CHECK_MIN_MAX(tmp_long,  -BALANCE, BALANCE);
1124
                        CHECK_MIN_MAX(tmp_long2, -BALANCE, BALANCE);
1125
                }
1126
                // correct current readings
1127
                ReadingIntegralGyroNick -= tmp_long;
1128
                ReadingIntegralGyroRoll -= tmp_long2;
1129
        }
1130
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1131
        // MeasurementCounter is incremented in the isr of analog.c
1132
        if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
1133
        {
1134
                static int16_t cnt = 0;
1135
                static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
1136
                static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old;
1137
 
1138
                // if not lopping in any direction (this should be always the case,
1139
                // because the Measurement counter is reset to 0 if looping in any direction is active.)
1140
                if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp)
1141
                {
1142
                        // Calculate mean value of the gyro integrals
1143
                        MeanIntegralGyroNick /= BALANCE_NUMBER;
1144
                        MeanIntegralGyroRoll /= BALANCE_NUMBER;
1145
 
1146
                        // Calculate mean of the acceleration values scaled to the gyro integrals
1147
                        MeanAccNick = (ParamSet.GyroAccFactor * MeanAccNick) / BALANCE_NUMBER;
1148
                        MeanAccRoll = (ParamSet.GyroAccFactor * MeanAccRoll) / BALANCE_NUMBER;
1149
 
1150
                        // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
1151
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
1152
                        IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick);
1153
                        CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim;
1154
                        AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
1155
                        // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
1156
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
1157
                        IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll);
1158
                        CorrectionRoll  = IntegralGyroRollError / ParamSet.GyroAccTrim;
1159
                        AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
1160
 
1161
                        if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.KalmanK == -1) )
1162
                        {
1163
                                AttitudeCorrectionNick /= 2;
1164
                                AttitudeCorrectionRoll /= 2;
1165
                        }
1166
 
1167
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1168
        // Gyro-Drift ermitteln
1169
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1170
                        // deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor)
1171
                        IntegralGyroNickError  = IntegralGyroNick2 - IntegralGyroNick;
1172
                        ReadingIntegralGyroNick2 -= IntegralGyroNickError;
1173
                        // deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor)
1174
                        IntegralGyroRollError = IntegralGyroRoll2 - IntegralGyroRoll;
1175
                        ReadingIntegralGyroRoll2 -= IntegralGyroRollError;
1176
 
1177
                        if(ParamSet.DriftComp)
1178
                        {
1179
                                if(YawGyroDrift >  BALANCE_NUMBER/2) AdBiasGyroYaw++;
1180
                                if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
1181
                        }
1182
                        YawGyroDrift = 0;
1183
 
1184
                        #define ERROR_LIMIT0 (BALANCE_NUMBER / 2)
1185
                        #define ERROR_LIMIT1 (BALANCE_NUMBER * 2)
1186
                        #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
1187
                        #define MOVEMENT_LIMIT 20000
1188
        // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
1189
                        cnt = 1;
1190
                        if(IntegralGyroNickError > ERROR_LIMIT1) cnt = 4;
1191
                        CorrectionNick = 0;
1192
                        if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
1193
                        {
1194
                                if(IntegralGyroNickError >  ERROR_LIMIT2)
1195
                                {
1196
                                        if(last_n_p)
1197
                                        {
1198
                                                cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
1199
                                                CorrectionNick = IntegralGyroNickError / 8;
1200
                                                if(CorrectionNick > 5000) CorrectionNick = 5000;
1201
                                                AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
1202
                                        }
1203
                                        else last_n_p = 1;
1204
                                }
1205
                                else  last_n_p = 0;
1206
                                if(IntegralGyroNickError < -ERROR_LIMIT2)
1207
                                {
1208
                                        if(last_n_n)
1209
                                        {
1210
                                                cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
1211
                                                CorrectionNick = IntegralGyroNickError / 8;
1212
                                                if(CorrectionNick < -5000) CorrectionNick = -5000;
1213
                                                AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
1214
                                        }
1215
                                        else last_n_n = 1;
1216
                                }
1217
                                else  last_n_n = 0;
1218
                        }
1219
                        else
1220
                        {
1221
                                cnt = 0;
1222
                                BadCompassHeading = 1000;
1223
                        }
1224
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1225
                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
1226
                        // correct Gyro Offsets
1227
                        if(IntegralGyroNickError >  ERROR_LIMIT0)   BiasHiResGyroNick += cnt;
1228
                        if(IntegralGyroNickError < -ERROR_LIMIT0)   BiasHiResGyroNick -= cnt;
1229
 
1230
        // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
1231
                        cnt = 1;
1232
                        if(IntegralGyroRollError > ERROR_LIMIT1) cnt = 4;
1233
                        CorrectionRoll = 0;
1234
                        if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
1235
                        {
1236
                                if(IntegralGyroRollError >  ERROR_LIMIT2)
1237
                                {
1238
                                        if(last_r_p)
1239
                                        {
1240
                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
1241
                                                CorrectionRoll = IntegralGyroRollError / 8;
1242
                                                if(CorrectionRoll > 5000) CorrectionRoll = 5000;
1243
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
1244
                                        }
1245
                                        else last_r_p = 1;
1246
                                }
1247
                                else  last_r_p = 0;
1248
                                if(IntegralGyroRollError < -ERROR_LIMIT2)
1249
                                {
1250
                                        if(last_r_n)
1251
                                        {
1252
                                                cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
1253
                                                CorrectionRoll = IntegralGyroRollError / 8;
1254
                                                if(CorrectionRoll < -5000) CorrectionRoll = -5000;
1255
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
1256
                                        }
1257
                                        else last_r_n = 1;
1258
                                }
1259
                                else  last_r_n = 0;
1260
                        }
1261
                        else
1262
                        {
1263
                                cnt = 0;
1264
                                BadCompassHeading = 1000;
1265
                        }
1266
                        // correct Gyro Offsets
1267
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1268
                        if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
1269
                        if(IntegralGyroRollError >  ERROR_LIMIT0)   BiasHiResGyroRoll += cnt;
1270
                        if(IntegralGyroRollError < -ERROR_LIMIT0)   BiasHiResGyroRoll -= cnt;
1271
 
1272
                }
1273
                else // looping is active
1274
                {
1275
                        AttitudeCorrectionRoll  = 0;
1276
                        AttitudeCorrectionNick = 0;
1277
                        FunnelCourse = 0;
1278
                }
1279
 
1280
                // if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction
1281
                if(!GyroIFactor)
1282
                {
1283
                        AttitudeCorrectionRoll  = 0;
1284
                        AttitudeCorrectionNick = 0;
1285
                }
1286
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1287
                MeanIntegralGyroNick_old = MeanIntegralGyroNick;
1288
                MeanIntegralGyroRoll_old = MeanIntegralGyroRoll;
1289
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1290
                // reset variables used for next averaging
1291
                MeanAccNick = 0;
1292
                MeanAccRoll = 0;
1293
                MeanIntegralGyroNick = 0;
1294
                MeanIntegralGyroRoll = 0;
1295
                MeasurementCounter = 0;
1296
        } // end of averaging
1297
 
1298
 
1299
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1300
//  Yawing
1301
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1302
        if(abs(StickYaw) > 15 ) // yaw stick is activated
1303
        {
1304
                BadCompassHeading = 1000;
1305
                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
1306
                {
1307
                        UpdateCompassCourse = 1;
1308
                }
1309
        }
1310
        // exponential stick sensitivity in yawring rate
1311
        tmp_int  = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
1312
        tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
1313
        SetPointYaw = tmp_int;
1314
        // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
1315
        ReadingIntegralGyroYaw -= tmp_int;
1316
        // limit the effect
1317
        CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
1318
 
1319
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1320
//  Compass
1321
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1322
    // compass code is used if Compass option is selected
1323
        if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
1324
        {
1325
                int16_t w, v, r,correction, error;
1326
 
1327
                if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
1328
                {
1329
                        SetCompassCalState();
1330
                        #ifdef USE_KILLAGREG
1331
                        MM3_Calibrate();
1332
                        #endif
1333
                }
1334
                else
1335
                {
1336
                        #ifdef USE_KILLAGREG
1337
                        static uint8_t updCompass = 0;
1338
                        if (!updCompass--)
1339
                        {
1340
                                updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
1341
                                MM3_Heading();
1342
                        }
1343
                        #endif
1344
 
1345
                        // get maximum attitude angle
1346
                        w = abs(IntegralGyroNick / 512);
1347
                        v = abs(IntegralGyroRoll / 512);
1348
                        if(v > w) w = v;
1349
                        correction = w / 8 + 1;
1350
                        // calculate the deviation of the yaw gyro heading and the compass heading
1351
                        if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
1352
                        else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
1353
                        if(abs(GyroYaw) > 128) // spinning fast
1354
                        {
1355
                                error = 0;
1356
                        }
1357
                        if(!BadCompassHeading && w < 25)
1358
                        {
1359
                                YawGyroDrift += error;
1360
                                if(UpdateCompassCourse)
1361
                                {
1362
                                        BeepTime = 200;
1363
                                        YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
1364
                                        CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
1365
                                        UpdateCompassCourse = 0;
1366
                                }
1367
                        }
1368
                        YawGyroHeading += (error * 8) / correction;
1369
                        w = (w * FCParam.CompassYawEffect) / 32;
1370
                        w = FCParam.CompassYawEffect - w;
1371
                        if(w >= 0)
1372
                        {
1373
                                if(!BadCompassHeading)
1374
                                {
1375
                                        v = 64 + (MaxStickNick + MaxStickRoll) / 8;
1376
                                        // calc course deviation
1377
                                        r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
1378
                                        v = (r * w) / v; // align to compass course
1379
                                        // limit yaw rate
1380
                                        w = 3 * FCParam.CompassYawEffect;
1381
                                        if (v > w) v = w;
1382
                                        else if (v < -w) v = -w;
1383
                                        ReadingIntegralGyroYaw += v;
1384
                                }
1385
                                else
1386
                                { // wait a while
1387
                                        BadCompassHeading--;
1388
                                }
1389
                        }
1390
                        else
1391
                        {  // ignore compass at extreme attitudes for a while
1392
                                BadCompassHeading = 500;
1393
                        }
1394
                }
1395
        }
1396
 
1397
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
1398
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1399
//  GPS
1400
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1401
        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
1402
        {
1403
                GPS_Main();
1404
                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
1405
        }
1406
        else
1407
        {
1408
                GPSStickNick = 0;
1409
                GPSStickRoll = 0;
1410
        }
1411
        #endif
1412
 
1413
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1414
//  Debugwerte zuordnen
1415
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1416
        if(!TimerDebugOut--)
1417
        {
1418
                TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
1419
                DebugOut.Analog[0]  = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg
1420
                DebugOut.Analog[1]  = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg
1421
                DebugOut.Analog[2]  = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg
1422
                DebugOut.Analog[3]  = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg
1423
                DebugOut.Analog[4]  = GyroYaw;
1424
                DebugOut.Analog[5]  = ReadingHeight;
1425
                DebugOut.Analog[6]  = (ReadingIntegralTop / 512);
1426
                DebugOut.Analog[8]  = CompassHeading;
1427
                DebugOut.Analog[9]  = UBat;
1428
                DebugOut.Analog[10] = RC_Quality;
1429
                DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR;
1430
                DebugOut.Analog[19] = CompassCalState;
1431
        //      DebugOut.Analog[24] = GyroNick/2;
1432
        //      DebugOut.Analog[25] = GyroRoll/2;
1433
                DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
1434
        //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
1435
                DebugOut.Analog[30] = GPSStickNick;
1436
                DebugOut.Analog[31] = GPSStickRoll;
1437
        }
1438
 
1439
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1440
//  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
1441
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1442
 
1443
        #define TRIM_LIMIT 200
1444
        CHECK_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT);
1445
        CHECK_MIN_MAX(TrimRoll, -TRIM_LIMIT, TRIM_LIMIT);
1446
 
1447
        if(FunnelCourse)
1448
        {
1449
                IPartNick = 0;
1450
                IPartRoll = 0;
1451
        }
1452
 
1453
        if(! LoopingNick)
1454
        {
1455
                PPartNick = (IntegralGyroNick * GyroIFactor) / (44000 / STICK_GAIN); // P-Part
1456
        }
1457
        else
1458
        {
1459
                PPartNick = 0;
1460
        }
1461
        PDPartNick = PPartNick + (int32_t)((int32_t)GyroNick * GyroPFactor + (int32_t)TrimNick * 128L) / (256L / STICK_GAIN); //  +D-Part
1462
 
1463
        if(!LoopingRoll)
1464
        {
1465
                PPartRoll = (IntegralGyroRoll * GyroIFactor) / (44000 / STICK_GAIN); // P-Part
1466
        }
1467
        else
1468
        {
1469
                PPartRoll = 0;
1470
        }
1471
        PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor +  (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part
1472
 
1473
        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN));
1474
 
1475
        //DebugOut.Analog[21] = PDPartNick;
1476
        //DebugOut.Analog[22] = PDPartRoll;
1477
 
1478
        // limit control feedback
1479
        #define SENSOR_LIMIT  (4096 * 4)
1480
        CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
1481
        CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
1482
        CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
1483
 
1484
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1485
// all BL-Ctrl connected?
1486
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1487
        if(MissingMotor)
1488
        {
1489
                // if we are in the lift off condition
1490
                if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) )
1491
                ModelIsFlying = 1; // keep within lift off condition
1492
                GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of
1493
        }
1494
 
1495
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1496
// Height Control
1497
// The height control algorithm reduces the gas but does not increase the gas.
1498
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1499
 
1500
        GasMixFraction *= STICK_GAIN;
1501
 
1502
        // if height control is activated and no emergency landing is active
1503
        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
1504
        {
1505
                int tmp_int;
1506
                static uint8_t delay = 100;
1507
                // if height control is activated by an rc channel
1508
                if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
1509
                {       // check if parameter is less than activation threshold
1510
                        if(
1511
                                ( (ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && ( (FCParam.MaxHeight > 80) && (FCParam.MaxHeight < 140) ) )|| // for 3-state switch height control is only disabled in center position
1512
                                (!(ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && (FCParam.MaxHeight < 50) ) // for 2-State switch height control is disabled in lower position
1513
                        )
1514
                        {   //hight control not active
1515
                                if(!delay--)
1516
                                {
1517
                                        // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
1518
                                        if( (ReadingAirPressure > 1000) && (OCR0A < 255) )
1519
                                        {   // increase offset
1520
                                                if(OCR0A < 244)
1521
                                                {
1522
                                                        ExpandBaro -= 10;
1523
                                                        OCR0A = PressureSensorOffset - ExpandBaro;
1524
                                                }
1525
                                                else
1526
                                                {
1527
                                                        OCR0A = 254;
1528
                                                }
1529
                                                BeepTime = 300;
1530
                                        delay = 250;
1531
                                        }
1532
                                        // measurement of air pressure close to lower limit and
1533
                                        else if( (ReadingAirPressure < 100) && (OCR0A > 1) )
1534
                                        {   // decrease offset
1535
                                                if(OCR0A > 10)
1536
                                                {
1537
                                                        ExpandBaro += 10;
1538
                                                        OCR0A = PressureSensorOffset - ExpandBaro;
1539
                                                }
1540
                                                else
1541
                                                {
1542
                                                        OCR0A = 1;
1543
                                                }
1544
                                                BeepTime = 300;
1545
                                        delay = 250;
1546
                                        }
1547
                                        else
1548
                                        {
1549
                                                SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
1550
                                                HeightControlActive = 0; // disable height control
1551
                                                delay = 1;
1552
                                        }
1553
                                }
1554
                        }
1555
                        else
1556
                        {       //hight control not active
1557
                                HeightControlActive = 1; // enable height control
1558
                                delay = 200;
1559
                        }
1560
                }
1561
                else // no switchable height control
1562
                {
1563
                        SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
1564
                        HeightControlActive = 1;
1565
                }
1566
                // get current height
1567
                h = ReadingHeight;
1568
                // if current height is above the setpoint reduce gas
1569
                if((h > SetPointHeight) && HeightControlActive)
1570
                {
1571
                        // height difference -> P control part
1572
                        h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN);
1573
                        h = GasMixFraction - h; // reduce gas
1574
                        // height gradient --> D control part
1575
                        //h -= (HeightD * FCParam.HeightD) / (8 / STICK_GAIN);  // D control part
1576
                        h -= (HeightD) / (8 / STICK_GAIN);  // D control part
1577
                        // acceleration sensor effect
1578
                        tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
1579
                        if(tmp_int > 70 * STICK_GAIN)        tmp_int =   70 * STICK_GAIN;
1580
                        else if(tmp_int < -(70 * STICK_GAIN)) tmp_int = -(70 * STICK_GAIN);
1581
                        h -= tmp_int;
1582
                        // update height control gas
1583
                        HeightControlGas = (HeightControlGas*15 + h) / 16;
1584
                        // limit gas reduction
1585
                        if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
1586
                        {
1587
                                if(GasMixFraction >= ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = ParamSet.HeightMinGas * STICK_GAIN;
1588
                                // allows landing also if gas stick is reduced below min gas on height control
1589
                                if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
1590
                        }
1591
                        // limit gas to stick setting
1592
                        if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction;
1593
                        GasMixFraction = HeightControlGas;
1594
                }
1595
        }
1596
        // limit gas to parameter setting
1597
        if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN;
1598
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1599
// + Mixer and PI-Controller
1600
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1601
        DebugOut.Analog[7] = GasMixFraction;
1602
 
1603
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1604
// Yaw-Fraction
1605
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1606
    YawMixFraction = PDPartYaw - SetPointYaw * STICK_GAIN;     // yaw controller
1607
        #define MIN_YAWGAS (40 * STICK_GAIN)  // yaw also below this gas value
1608
        // limit YawMixFraction
1609
        if(GasMixFraction > MIN_YAWGAS)
1610
        {
1611
                CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
1612
        }
1613
        else
1614
        {
1615
                CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
1616
        }
1617
        tmp_int = ParamSet.GasMax * STICK_GAIN;
1618
        CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
1619
 
1620
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1621
// Nick-Axis
1622
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623
        DiffNick = PDPartNick - StickNick;      // get difference
1624
        if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control
1625
        else IPartNick += DiffNick; // I-part for head holding
1626
        CHECK_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
1627
        NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick
1628
 
1629
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1630
// Roll-Axis
1631
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1632
        DiffRoll = PDPartRoll - StickRoll;      // get difference
1633
        if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control
1634
        else IPartRoll += DiffRoll;  // I-part for head holding
1635
        CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
1636
        RollMixFraction = DiffRoll + (IPartRoll / Ki);   // PID-controller for roll
1637
 
1638
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1639
// Limiter
1640
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1641
        tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
1642
        CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
1643
        CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
1644
 
1645
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1646
// Universal Mixer
1647
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1648
        for(i = 0; i < MAX_MOTORS; i++)
1649
        {
1650
                int16_t tmp;
1651
                if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer
1652
                {
1653
                        tmp =  ((int32_t)GasMixFraction  * Mixer.Motor[i][MIX_GAS] ) / 64L;
1654
                        tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L;
1655
                        tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
1656
                        tmp += ((int32_t)YawMixFraction  * Mixer.Motor[i][MIX_YAW] ) / 64L;
1657
                        MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]);  // Spike Filter
1658
                        tmp = MotorValue[i] / STICK_GAIN;
1659
                        CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
1660
                        Motor[i].SetPoint = tmp;
1661
                }
1662
                else Motor[i].SetPoint = 0;
1663
        }
1664
}
1665