| 112,10 → 112,17 |
|---|
| 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] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| int32_t tmp; |
| |
| tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| if (reverse[PITCH]) target[PITCH] -= tmp; else target[PITCH] += tmp; |
| |
| tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| if (reverse[ROLL]) target[ROLL] -= tmp; else target[ROLL] += tmp; |
| |
| tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| if (reverse[YAW]) target[YAW] -= tmp; else target[YAW] += tmp; |
| |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| target[axis] -= OVER360; |
| 123,10 → 130,10 |
|---|
| target[axis] += OVER360; |
| } |
| |
| if (reverse[axis]) |
| error[axis] = target[axis] - attitude[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| //if (reverse[axis]) |
| error[axis] = attitude[axis] - target[axis]; |
| // else |
| // error[axis] = attitude[axis] - target[axis]; |
| |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| 141,10 → 148,10 |
|---|
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
| PDPart[axis] = (((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| //if (reverse[axis]) |
| // PDPart[axis] = -PDPart[axis]; |
| } else { |
| PDPart[axis] = 0; |
| } |
| 151,14 → 158,17 |
|---|
| |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| // if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| // else |
| // PDPart[axis] -= anglePart; |
| } |
| |
| // Add I parts here... these are integrated errors. |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| if (reverse[axis]) |
| term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
| else |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| } |
| |
| debugOut.analog[12] = term[PITCH]; |