Subversion Repositories FlightCtrl

Rev

Rev 1221 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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