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 |