Rev 2119 | Rev 2126 | Go to most recent revision | Show entire file | Regard 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; |
- | |
124 | } |
- | |
125 | 130 | target[axis] += OVER360; |
|
- | 131 | } |
|
- | 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 | } |