Rev 2073 | Rev 2086 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2073 | Rev 2085 | ||
|---|---|---|---|
| Line 24... | Line 24... | ||
| 24 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
24 | * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
| 25 | */ |
25 | */ |
| 26 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
26 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| 27 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
27 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| 28 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
28 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
| 29 | uint8_t invKi; |
29 | uint8_t ki; |
| 30 | int32_t IPart[2]; |
30 | int32_t IPart[2]; |
| Line 31... | Line 31... | ||
| 31 | 31 | ||
| 32 | /************************************************************************/ |
32 | /************************************************************************/ |
| 33 | /* Filter for motor value smoothing (necessary???) */ |
33 | /* Filter for motor value smoothing (necessary???) */ |
| Line 51... | Line 51... | ||
| 51 | default: |
51 | default: |
| 52 | return newvalue; |
52 | return newvalue; |
| 53 | } |
53 | } |
| 54 | } |
54 | } |
| Line 55... | Line 55... | ||
| 55 | 55 | ||
| 56 | void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor, |
56 | void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor, |
| 57 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
57 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| 58 | invKi = _invKi; |
58 | ki = _ki; |
| 59 | gyroPFactor = _gyroPFactor; |
59 | gyroPFactor = _gyroPFactor; |
| 60 | gyroIFactor = _gyroIFactor; |
60 | gyroIFactor = _gyroIFactor; |
| 61 | yawPFactor = _yawPFactor; |
61 | yawPFactor = _yawPFactor; |
| 62 | yawIFactor = _yawIFactor; |
62 | yawIFactor = _yawIFactor; |
| Line 213... | Line 213... | ||
| 213 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
213 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
| 214 | } |
214 | } |
| Line 215... | Line 215... | ||
| 215 | 215 | ||
| Line 216... | Line 216... | ||
| 216 | PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
216 | PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| 217 | 217 | ||
| Line 218... | Line 218... | ||
| 218 | term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14); |
218 | term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14); |
| 219 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
219 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
| 220 | 220 | ||