| 172,7 → 172,7 |
|---|
| // We want that to kick as early as possible, not to delay new AD sampling further. |
| calculateFlightAttitude(); |
| controlMixer_update(); |
| throttleTerm = controlThrottle; |
| throttleTerm = controls[CONTROL_THROTTLE]; |
| |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| if (throttleTerm < staticParams.MinThrottle + 10) |
| 261,7 → 261,7 |
|---|
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| 273,7 → 273,7 |
|---|
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| yawAngleDiff -= controls[CONTROL_YAW]; |
| |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| 353,7 → 353,7 |
|---|
| * between current throttle and maximum throttle). |
| */ |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
| // Limit yawTerm |
| DebugOut.Digital[0] &= ~DEBUG_CLIP; |
| if (throttleTerm > MIN_YAWGAS) { |
| 396,11 → 396,11 |
|---|
| // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
| // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
| // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
| IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| } else { |
| // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
| // To keep up with a full stick PDPart should be about 156... |
| IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| } |
| |
| tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
| 409,7 → 409,7 |
|---|
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| 432,37 → 432,41 |
|---|
| |
| DebugOut.Analog[12] = term[PITCH]; |
| DebugOut.Analog[13] = term[ROLL]; |
| DebugOut.Analog[14] = yawTerm; |
| //DebugOut.Analog[14] = yawTerm; |
| DebugOut.Analog[15] = throttleTerm; |
| |
| for (i = 0; i < MAX_MOTORS; i++) { |
| int32_t tmp; |
| uint8_t throttle; |
| |
| tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
| tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
| tmp = tmp >> 6; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / MOTOR_SCALING; |
| |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, 1, 255); |
| throttle = tmp; |
| |
| if (i < 4) DebugOut.Analog[22 + i] = throttle; |
| |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
| tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
| tmp = tmp >> 6; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / MOTOR_SCALING; |
| |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, 1, 255); |
| motor[i].SetPoint = tmp; |
| motor[i].SetPoint = throttle; |
| } else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| } |
| if (i < 4) |
| DebugOut.Analog[22 + i] = motor[i].SetPoint; |
| } |
| |
| I2C_Start(TWI_STATE_MOTOR_TX); |