107,9 → 107,9 |
term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
|
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
|
for (axis = PITCH; axis <= YAW; axis++) { |
if (target[axis] > OVER180) { |
127,6 → 127,8 |
error[axis] = maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] =- maxError[axis]; |
} else { |
// update I parts here for angles mode. Ĩ parts in rate mode is something different. |
} |
|
#define LOG_P_SCALE 6 |
158,9 → 160,8 |
else |
PDPart[axis] -= anglePart; |
} |
|
// Add I parts here... these are integrated errors. |
// When an error wraps, actually its I part should be negated or something... |
|
term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
} |
|