Rev 1922 | Rev 2025 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1922 | Rev 1927 | ||
|---|---|---|---|
| Line 154... | Line 154... | ||
| 154 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
154 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
| Line 155... | Line 155... | ||
| 155 | 155 | ||
| 156 | /************************************************************************/ |
156 | /************************************************************************/ |
| 157 | /* Stick signals are positive and gyros are negative... */ |
157 | /* Stick signals are positive and gyros are negative... */ |
| 158 | /************************************************************************/ |
158 | /************************************************************************/ |
| 159 | IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
- | |
| 160 | if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
- | |
| 161 | else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |
- | |
| 162 | if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE; |
- | |
| 163 | else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE; |
- | |
| 164 | - | ||
| 165 | IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL]; |
- | |
| 166 | if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360; |
- | |
| 167 | else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360; |
159 | IPart[PITCH] = error[PITCH]; // * some factor configurable. |
| 168 | if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE; |
160 | IPart[ROLL] = error[ROLL]; |
| 169 | else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE; |
- | |
| 170 | 161 | // TODO: Add ipart. Or add/subtract depending, not sure. |
|
| 171 | term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]); |
162 | term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]); |
| 172 | term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]); |
163 | term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]); |
| Line 173... | Line -... | ||
| 173 | yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw); |
- | |
| 174 | 164 | yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw); |
|
| 175 | 165 | ||
| 176 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
166 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 177 | // Universal Mixer |
167 | // Universal Mixer |
| Line 208... | Line 198... | ||
| 208 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
198 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 209 | if (!(--debugDataTimer)) { |
199 | if (!(--debugDataTimer)) { |
| 210 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
200 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| 211 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
201 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 212 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
202 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 213 | DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
203 | DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW; |
| Line 214... | Line 204... | ||
| 214 | 204 | ||
| 215 | DebugOut.Analog[6] = pitchPFactor; |
205 | DebugOut.Analog[6] = pitchPFactor; |
| 216 | DebugOut.Analog[7] = rollPFactor; |
206 | DebugOut.Analog[7] = rollPFactor; |
| 217 | DebugOut.Analog[8] = yawPFactor; |
207 | DebugOut.Analog[8] = yawPFactor; |
| 218 | DebugOut.Analog[9] = pitchDFactor; |
208 | DebugOut.Analog[9] = pitchDFactor; |
| 219 | DebugOut.Analog[10] = rollDFactor; |
209 | DebugOut.Analog[10] = rollDFactor; |
| Line 220... | Line 210... | ||
| 220 | DebugOut.Analog[11] = yawDFactor; |
210 | DebugOut.Analog[11] = yawDFactor; |
| 221 | 211 | ||
| 222 | DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
212 | DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 223 | DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
213 | DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 224 | DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
214 | DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 225 | DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
215 | DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |