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); |