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