| 22,7 → 22,6 |
|---|
| /* |
| * Error integrals. |
| */ |
| int32_t error[3]; |
| |
| uint8_t reverse[3]; |
| int32_t maxError[3]; |
| 45,6 → 44,7 |
|---|
| |
| void flight_updateFlightParametersToFlightMode(void) { |
| debugOut.analog[16] = currentFlightMode; |
| |
| reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
| 130,58 → 130,61 |
|---|
| target[axis] += OVER360; |
| } |
| |
| error[axis] = attitude[axis] - target[axis]; |
| int32_t error = attitude[axis] - target[axis]; |
| |
| #define ROTATETARGET 1 |
| // #define TRUNCATEERROR 1 |
| |
| #ifdef ROTATETARGET |
| if(abs(error[axis]) > OVER180) { |
| //if(abs(error) > OVER180) { // doesnt work!!! |
| if(error > OVER180 || error < -OVER180) { |
| // The shortest way from attitude to target crosses -180. |
| // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
| // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
| if (error[axis] > 0) { |
| if (error[axis] < OVER360 - maxError[axis]) { |
| if (error > 0) { |
| if (error < OVER360 - maxError[axis]) { |
| // too much err. |
| error[axis] = -maxError[axis]; |
| error = -maxError[axis]; |
| target[axis] = attitude[axis] + maxError[axis]; |
| if (target[axis] > OVER180) target[axis] -= OVER360; |
| } else { |
| // Normal case, we just need to correct for the wrap. Error will be negative. |
| error[axis] -= OVER360; |
| error -= OVER360; |
| } |
| } else { |
| if (error[axis] > maxError[axis] - OVER360) { |
| if (error > maxError[axis] - OVER360) { |
| // too much err. |
| error[axis] = maxError[axis]; |
| error = maxError[axis]; |
| target[axis] = attitude[axis] - maxError[axis]; |
| if (target[axis] < -OVER180) target[axis] += OVER360; |
| } else { |
| // Normal case, we just need to correct for the wrap. Error will be negative. |
| error[axis] += OVER360; |
| error += OVER360; |
| } |
| } |
| } else { |
| // Simple case, linear range. |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| if (error > maxError[axis]) { |
| error = maxError[axis]; |
| target[axis] = attitude[axis] - maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] = -maxError[axis]; |
| } else if (error < -maxError[axis]) { |
| error = -maxError[axis]; |
| target[axis] = attitude[axis] + maxError[axis]; |
| } |
| } |
| #endif |
| #ifdef TUNCATEERROR |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] = -maxError[axis]; |
| if (error > maxError[axis]) { |
| error = maxError[axis]; |
| } else if (error < -maxError[axis]) { |
| error = -maxError[axis]; |
| } else { |
| // update I parts here for angles mode. I parts in rate mode is something different. |
| } |
| #endif |
| |
| debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| 196,7 → 199,7 |
|---|
| } |
| |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| PDPart[axis] += anglePart; |
| } |
| |
| 258,10 → 261,6 |
|---|
| debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
| |
| debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
| |
| debugOut.analog[12] = term[PITCH]; |
| debugOut.analog[13] = term[ROLL]; |
| debugOut.analog[14] = term[YAW]; |