Rev 2119 | Rev 2126 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2119 | Rev 2122 | ||
|---|---|---|---|
| Line 110... | Line 110... | ||
| 110 | setAirspeedPIDs(); |
110 | setAirspeedPIDs(); |
| Line 111... | Line 111... | ||
| 111 | 111 | ||
| Line 112... | Line 112... | ||
| 112 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
112 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| - | 113 | ||
| - | 114 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
|
| 113 | 115 | int32_t tmp; |
|
| - | 116 | ||
| - | 117 | tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
|
| 114 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
118 | if (reverse[PITCH]) target[PITCH] -= tmp; else target[PITCH] += tmp; |
| - | 119 | ||
| - | 120 | tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
|
| 115 | target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
121 | if (reverse[ROLL]) target[ROLL] -= tmp; else target[ROLL] += tmp; |
| - | 122 | ||
| Line 116... | Line 123... | ||
| 116 | target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
123 | tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| 117 | target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
124 | if (reverse[YAW]) target[YAW] -= tmp; else target[YAW] += tmp; |
| 118 | 125 | ||
| 119 | for (axis = PITCH; axis <= YAW; axis++) { |
126 | for (axis = PITCH; axis <= YAW; axis++) { |
| 120 | if (target[axis] > OVER180) { |
127 | if (target[axis] > OVER180) { |
| 121 | target[axis] -= OVER360; |
128 | target[axis] -= OVER360; |
| Line 122... | Line 129... | ||
| 122 | } else if (target[axis] <= -OVER180) { |
129 | } else if (target[axis] <= -OVER180) { |
| 123 | target[axis] += OVER360; |
130 | target[axis] += OVER360; |
| 124 | } |
131 | } |
| 125 | 132 | ||
| Line 126... | Line 133... | ||
| 126 | if (reverse[axis]) |
133 | //if (reverse[axis]) |
| 127 | error[axis] = target[axis] - attitude[axis]; |
134 | error[axis] = attitude[axis] - target[axis]; |
| 128 | else |
135 | // else |
| 129 | error[axis] = attitude[axis] - target[axis]; |
136 | // error[axis] = attitude[axis] - target[axis]; |
| Line 139... | Line 146... | ||
| 139 | /************************************************************************/ |
146 | /************************************************************************/ |
| 140 | /* Calculate control feedback from angle (gyro integral) */ |
147 | /* Calculate control feedback from angle (gyro integral) */ |
| 141 | /* and angular velocity (gyro signal) */ |
148 | /* and angular velocity (gyro signal) */ |
| 142 | /************************************************************************/ |
149 | /************************************************************************/ |
| 143 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
150 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
| 144 | PDPart[axis] = (((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
151 | PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| 145 | + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
152 | + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
| 146 | if (reverse[axis]) |
153 | //if (reverse[axis]) |
| 147 | PDPart[axis] = -PDPart[axis]; |
154 | // PDPart[axis] = -PDPart[axis]; |
| 148 | } else { |
155 | } else { |
| 149 | PDPart[axis] = 0; |
156 | PDPart[axis] = 0; |
| 150 | } |
157 | } |
| Line 151... | Line 158... | ||
| 151 | 158 | ||
| 152 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
159 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| 153 | int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
160 | int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| 154 | if (reverse[axis]) |
161 | // if (reverse[axis]) |
| 155 | PDPart[axis] += anglePart; |
162 | PDPart[axis] += anglePart; |
| 156 | else |
163 | // else |
| 157 | PDPart[axis] -= anglePart; |
164 | // PDPart[axis] -= anglePart; |
| Line 158... | Line 165... | ||
| 158 | } |
165 | } |
| - | 166 | ||
| - | 167 | // Add I parts here... these are integrated errors. |
|
| - | 168 | if (reverse[axis]) |
|
| 159 | 169 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
|
| 160 | // Add I parts here... these are integrated errors. |
170 | else |
| Line 161... | Line 171... | ||
| 161 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
171 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| 162 | } |
172 | } |
| 163 | 173 | ||