Subversion Repositories FlightCtrl

Rev

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

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