Rev 1821 | Rev 1844 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1821 | Rev 1841 | ||
---|---|---|---|
Line 79... | Line 79... | ||
79 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
79 | * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
80 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
80 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
81 | */ |
81 | */ |
82 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
82 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
Line 83... | Line 83... | ||
83 | 83 | ||
84 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
84 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
Line 85... | Line 85... | ||
85 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
85 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
86 | 86 | ||
87 | // Some integral weight constant... |
87 | // Some integral weight constant... |
Line 93... | Line 93... | ||
93 | 93 | ||
94 | /************************************************************************/ |
94 | /************************************************************************/ |
95 | /* Filter for motor value smoothing (necessary???) */ |
95 | /* Filter for motor value smoothing (necessary???) */ |
96 | /************************************************************************/ |
96 | /************************************************************************/ |
97 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
97 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
98 | switch (dynamicParams.UserParams[5]) { |
98 | switch(dynamicParams.UserParams[5]) { |
99 | case 0: |
99 | case 0: |
100 | return newvalue; |
100 | return newvalue; |
101 | case 1: |
101 | case 1: |
102 | return (oldvalue + newvalue) / 2; |
102 | return (oldvalue + newvalue) / 2; |
103 | case 2: |
103 | case 2: |
104 | if (newvalue > oldvalue) |
104 | if(newvalue > oldvalue) |
105 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
105 | return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
106 | else |
106 | else |
107 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
107 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
108 | case 3: |
108 | case 3: |
109 | if (newvalue < oldvalue) |
109 | if(newvalue < oldvalue) |
110 | return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
110 | return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
111 | else |
111 | else |
112 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
- | |
113 | default: |
112 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
114 | return newvalue; |
113 | default: return newvalue; |
115 | } |
114 | } |
Line 116... | Line 115... | ||
116 | } |
115 | } |
117 | 116 | ||
118 | /************************************************************************/ |
117 | /************************************************************************/ |
119 | /* Neutral Readings */ |
118 | /* Neutral Readings */ |
120 | /************************************************************************/ |
119 | /************************************************************************/ |
Line 121... | Line 120... | ||
121 | void flight_setNeutral() { |
120 | void flight_setNeutral() { |
122 | MKFlags |= MKFLAG_CALIBRATE; |
121 | MKFlags |= MKFLAG_CALIBRATE; |
123 | 122 | ||
124 | // not really used here any more. |
123 | // not really used here any more. |
Line 125... | Line 124... | ||
125 | dynamicParams.KalmanK = -1; |
124 | dynamicParams.KalmanK = -1; |
126 | dynamicParams.KalmanMaxDrift = 0; |
125 | dynamicParams.KalmanMaxDrift = 0; |
Line 127... | Line -... | ||
127 | dynamicParams.KalmanMaxFusion = 32; |
- | |
128 | 126 | dynamicParams.KalmanMaxFusion = 32; |
|
129 | controlMixer_initVariables(); |
127 | |
130 | } |
128 | controlMixer_initVariables(); |
131 | 129 | } |
|
132 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
130 | |
133 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
131 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
134 | Ki = 10300 / _Ki; |
132 | Ki = 10300 / _Ki; |
Line 135... | Line 133... | ||
135 | gyroPFactor = _gyroPFactor; |
133 | gyroPFactor = _gyroPFactor; |
136 | gyroIFactor = _gyroIFactor; |
134 | gyroIFactor = _gyroIFactor; |
- | 135 | yawPFactor = _yawPFactor; |
|
137 | yawPFactor = _yawPFactor; |
136 | yawIFactor = _yawIFactor; |
- | 137 | } |
|
138 | yawIFactor = _yawIFactor; |
138 | |
- | 139 | void setNormalFlightParameters(void) { |
|
139 | } |
140 | setFlightParameters(dynamicParams.IFactor + 1, |
Line 140... | Line 141... | ||
140 | 141 | dynamicParams.GyroP + 10, |
|
141 | void setNormalFlightParameters(void) { |
142 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
142 | setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
143 | dynamicParams.GyroP + 10, |
Line -... | Line 144... | ||
- | 144 | dynamicParams.UserParams[6] |
|
143 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
145 | ); |
144 | dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
146 | } |
145 | } |
147 | |
146 | 148 | void setStableFlightParameters(void) { |
|
283 | 283 | ||
284 | /************************************************************************/ |
284 | /************************************************************************/ |
285 | /* Compass is currently not supported. */ |
285 | /* Compass is currently not supported. */ |
286 | /************************************************************************/ |
286 | /************************************************************************/ |
287 | if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
287 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
288 | updateCompass(); |
288 | updateCompass(); |
289 | } |
289 | } |
290 | 290 | ||
291 | #if defined (USE_NAVICTRL) |
- | |
292 | /************************************************************************/ |
- | |
293 | /* GPS is currently not supported. */ |
291 | #if defined (USE_NAVICTRL) |
294 | /************************************************************************/ |
292 | /************************************************************************/ |
295 | if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
293 | /* GPS is currently not supported. */ |
296 | GPS_Main(); |
294 | /************************************************************************/ |
297 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
295 | if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
298 | } else { |
296 | GPS_Main(); |
Line 299... | Line 297... | ||
299 | // GPSStickPitch = 0; |
297 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
300 | // GPSStickRoll = 0; |
298 | } else { |
301 | } |
299 | } |
302 | #endif |
300 | #endif |
303 | // end part 1: 750-800 usec. |
301 | // end part 1: 750-800 usec. |
304 | // start part 3: 350 - 400 usec. |
302 | // start part 3: 350 - 400 usec. |
305 | #define SENSOR_LIMIT (4096 * 4) |
303 | #define SENSOR_LIMIT (4096 * 4) |
306 | /************************************************************************/ |
304 | /************************************************************************/ |
307 | 305 | ||
308 | /* Calculate control feedback from angle (gyro integral) */ |
306 | /* Calculate control feedback from angle (gyro integral) */ |
309 | /* and angular velocity (gyro signal) */ |
307 | /* and angular velocity (gyro signal) */ |
310 | /************************************************************************/ |
308 | /************************************************************************/ |
311 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
309 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
312 | for (axis = PITCH; axis <= ROLL; axis++) { |
310 | for (axis=PITCH; axis<=ROLL; axis++) { |
313 | if (looping & ((1 << 4) << axis)) { |
311 | if(looping & ((1<<4)<<axis)) { |
314 | PPart[axis] = 0; |
312 | PPart[axis] = 0; |
315 | } else { // TODO: Where do the 44000 come from??? |
313 | } else { // TODO: Where do the 44000 come from??? |
316 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
- | |
317 | } |
314 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
318 | 315 | } |
|
319 | /* |
316 | |
320 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
317 | /* |
321 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
318 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
- | 319 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
|
322 | * where pfactor is in [0..1]. |
320 | * where pfactor is in [0..1]. |
323 | */ |
321 | */ |
324 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
- | |
325 | * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
322 | PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
326 | * (int16_t) dynamicParams.GyroD) / 16; |
323 | + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
327 | 324 | ||
328 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
325 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
329 | } |
326 | } |
330 | 327 | ||
331 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
328 | PDPartYaw = |
332 | / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
329 | (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
333 | / CONTROL_SCALING)); |
330 | + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
334 | 331 | ||
335 | // limit control feedback |
332 | // limit control feedback |
336 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
333 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
337 | 334 | ||
338 | /* |
335 | /* |
339 | * Compose throttle term. |
336 | * Compose throttle term. |
340 | * If a Bl-Ctrl is missing, prevent takeoff. |
337 | * If a Bl-Ctrl is missing, prevent takeoff. |
341 | */ |
338 | */ |
342 | if (missingMotor) { |
339 | if(missingMotor) { |
343 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
340 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
344 | if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
341 | if(isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
345 | isFlying = 1; // keep within lift off condition |
342 | isFlying = 1; // keep within lift off condition |
346 | throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
343 | throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
347 | } |
344 | } |
348 | 345 | ||
349 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
346 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
350 | throttleTerm *= CONTROL_SCALING; |
347 | throttleTerm *= CONTROL_SCALING; |
454 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
480 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |