Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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