| 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]; |
| } |
| |