Rev 1805 | Rev 1844 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1805 | Rev 1821 | ||
---|---|---|---|
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 |
|
112 | return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
113 | default: |
113 | default: return newvalue; |
114 | return newvalue; |
114 | } |
115 | } |
Line 115... | Line 116... | ||
115 | } |
116 | } |
116 | 117 | ||
117 | /************************************************************************/ |
118 | /************************************************************************/ |
118 | /* Neutral Readings */ |
119 | /* Neutral Readings */ |
119 | /************************************************************************/ |
120 | /************************************************************************/ |
Line 120... | Line 121... | ||
120 | void flight_setNeutral() { |
121 | void flight_setNeutral() { |
121 | MKFlags |= MKFLAG_CALIBRATE; |
122 | MKFlags |= MKFLAG_CALIBRATE; |
122 | 123 | ||
123 | // not really used here any more. |
124 | // not really used here any more. |
Line 124... | Line 125... | ||
124 | dynamicParams.KalmanK = -1; |
125 | dynamicParams.KalmanK = -1; |
125 | dynamicParams.KalmanMaxDrift = 0; |
126 | dynamicParams.KalmanMaxDrift = 0; |
Line -... | Line 127... | ||
- | 127 | dynamicParams.KalmanMaxFusion = 32; |
|
126 | dynamicParams.KalmanMaxFusion = 32; |
128 | |
127 | 129 | controlMixer_initVariables(); |
|
128 | controlMixer_initVariables(); |
130 | } |
129 | } |
131 | |
130 | 132 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
|
131 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
133 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
132 | Ki = 10300 / _Ki; |
134 | Ki = 10300 / _Ki; |
Line 133... | Line 135... | ||
133 | gyroPFactor = _gyroPFactor; |
135 | gyroPFactor = _gyroPFactor; |
134 | gyroIFactor = _gyroIFactor; |
136 | gyroIFactor = _gyroIFactor; |
135 | yawPFactor = _yawPFactor; |
- | |
136 | yawIFactor = _yawIFactor; |
137 | yawPFactor = _yawPFactor; |
137 | } |
- | |
138 | 138 | yawIFactor = _yawIFactor; |
|
139 | void setNormalFlightParameters(void) { |
- | |
140 | setFlightParameters(dynamicParams.IFactor + 1, |
139 | } |
Line 141... | Line 140... | ||
141 | dynamicParams.GyroP + 10, |
140 | |
142 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
141 | void setNormalFlightParameters(void) { |
143 | dynamicParams.GyroP + 10, |
142 | setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
Line 144... | Line -... | ||
144 | dynamicParams.UserParams[6] |
- | |
145 | ); |
143 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
146 | } |
144 | dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
147 | 145 | } |
|
148 | void setStableFlightParameters(void) { |
146 | |
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) |
291 | #if defined (USE_NAVICTRL) |
292 | /************************************************************************/ |
292 | /************************************************************************/ |
293 | /* GPS is currently not supported. */ |
293 | /* GPS is currently not supported. */ |
294 | /************************************************************************/ |
294 | /************************************************************************/ |
295 | if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
295 | if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
296 | GPS_Main(); |
296 | GPS_Main(); |
297 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
297 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
298 | } else { |
298 | } else { |
Line 299... | Line 299... | ||
299 | // GPSStickPitch = 0; |
299 | // GPSStickPitch = 0; |
300 | // GPSStickRoll = 0; |
300 | // GPSStickRoll = 0; |
301 | } |
301 | } |
302 | #endif |
302 | #endif |
303 | // end part 1: 750-800 usec. |
303 | // end part 1: 750-800 usec. |
304 | // start part 3: 350 - 400 usec. |
304 | // start part 3: 350 - 400 usec. |
305 | #define SENSOR_LIMIT (4096 * 4) |
305 | #define SENSOR_LIMIT (4096 * 4) |
306 | /************************************************************************/ |
306 | /************************************************************************/ |
307 | 307 | ||
308 | /* Calculate control feedback from angle (gyro integral) */ |
308 | /* Calculate control feedback from angle (gyro integral) */ |
309 | /* and angular velocity (gyro signal) */ |
309 | /* and angular velocity (gyro signal) */ |
310 | /************************************************************************/ |
310 | /************************************************************************/ |
311 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
311 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
312 | for (axis=PITCH; axis<=ROLL; axis++) { |
312 | for (axis = PITCH; axis <= ROLL; axis++) { |
313 | if(looping & ((1<<4)<<axis)) { |
313 | if (looping & ((1 << 4) << axis)) { |
314 | PPart[axis] = 0; |
314 | PPart[axis] = 0; |
315 | } else { // TODO: Where do the 44000 come from??? |
315 | } else { // TODO: Where do the 44000 come from??? |
- | 316 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
|
316 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
317 | } |
317 | } |
318 | |
318 | 319 | /* |
|
319 | /* |
320 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
320 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
321 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
321 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
- | |
322 | * where pfactor is in [0..1]. |
322 | * where pfactor is in [0..1]. |
323 | */ |
323 | */ |
- | 324 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
|
324 | PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
325 | * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
325 | + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
326 | * (int16_t) dynamicParams.GyroD) / 16; |
326 | 327 | ||
327 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
328 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
328 | } |
329 | } |
329 | 330 | ||
330 | PDPartYaw = |
331 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
331 | (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
332 | / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
332 | + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
333 | / CONTROL_SCALING)); |
333 | 334 | ||
334 | // limit control feedback |
335 | // limit control feedback |
335 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
336 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
336 | 337 | ||
337 | /* |
338 | /* |
338 | * Compose throttle term. |
339 | * Compose throttle term. |
339 | * If a Bl-Ctrl is missing, prevent takeoff. |
340 | * If a Bl-Ctrl is missing, prevent takeoff. |
340 | */ |
341 | */ |
341 | if(missingMotor) { |
342 | if (missingMotor) { |
342 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
343 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
343 | if(isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
344 | if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
344 | isFlying = 1; // keep within lift off condition |
345 | isFlying = 1; // keep within lift off condition |
345 | throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
346 | throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
346 | } |
347 | } |
347 | 348 | ||
348 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
349 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
349 | throttleTerm *= CONTROL_SCALING; |
350 | throttleTerm *= CONTROL_SCALING; |
453 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
454 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |