| 26,7 → 26,7 |
|---|
| // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
| uint8_t invKi; |
| uint8_t ki; |
| int32_t IPart[2]; |
| |
| /************************************************************************/ |
| 53,9 → 53,9 |
|---|
| } |
| } |
| |
| void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor, |
| void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor, |
| uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| invKi = _invKi; |
| ki = _ki; |
| gyroPFactor = _gyroPFactor; |
| gyroIFactor = _gyroIFactor; |
| yawPFactor = _yawPFactor; |
| 215,7 → 215,7 |
|---|
| |
| PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| |
| term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14); |
| term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14); |
| term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
| |
| /* |