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