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); |