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