110,8 → 110,7 |
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
else |
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
default: |
return newvalue; |
default: return newvalue; |
} |
} |
|
129,8 → 128,7 |
controlMixer_initVariables(); |
} |
|
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Ki = 10300 / _Ki; |
gyroPFactor = _gyroPFactor; |
gyroIFactor = _gyroIFactor; |
139,9 → 137,12 |
} |
|
void setNormalFlightParameters(void) { |
setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
setFlightParameters(dynamicParams.IFactor + 1, |
dynamicParams.GyroP + 10, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
dynamicParams.GyroP + 10, |
dynamicParams.UserParams[6] |
); |
} |
|
void setStableFlightParameters(void) { |
148,6 → 149,7 |
setFlightParameters(33, 90, 120, 90, 120); |
} |
|
|
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
180,8 → 182,8 |
|
throttleTerm = controlThrottle; |
// This check removed. Is done on a per-motor basis, after output matrix multiplication. |
// if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
// else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
|
/************************************************************************/ |
/* RC-signal is bad */ |
220,8 → 222,7 |
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
// increment flight-time counter until overflow. |
if (isFlying != 0xFFFF) |
isFlying++; |
if(isFlying != 0xFFFF) isFlying++; |
} else |
/* |
* When standing on the ground, do not apply I controls and zero the yaw stick. |
257,8 → 258,7 |
* This is the throttle part. |
*/ |
if (looping) { |
if (throttleTerm > staticParams.LoopGasLimit) |
throttleTerm = staticParams.LoopGasLimit; |
if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
} |
|
/************************************************************************/ |
296,8 → 296,6 |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} else { |
// GPSStickPitch = 0; |
// GPSStickRoll = 0; |
} |
#endif |
// end part 1: 750-800 usec. |
321,16 → 319,15 |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
* gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
* (int16_t) dynamicParams.GyroD) / 16; |
PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
+ (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
|
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
|
PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
/ CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
/ CONTROL_SCALING)); |
PDPartYaw = |
(int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
+ (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
|
// limit control feedback |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
359,18 → 356,38 |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
// Limit yawTerm |
DebugOut.Digital[0] &= ~DEBUG_CLIP; |
if (throttleTerm > MIN_YAWGAS) { |
CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
if (yawTerm < -throttleTerm/2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = -throttleTerm/2; |
} else if (yawTerm > throttleTerm/2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = throttleTerm/2; |
} |
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
if (yawTerm < -MIN_YAWGAS/2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = -MIN_YAWGAS/2; |
} else if (yawTerm > MIN_YAWGAS/2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
yawTerm = MIN_YAWGAS/2; |
} |
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
|
// FIXME: Throttle may exceed maxThrottle (there is no check no more). |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
|
tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
|
if (yawTerm < -(tmpInt - throttleTerm)) { |
yawTerm = -(tmpInt - throttleTerm); |
DebugOut.Digital[0] |= DEBUG_CLIP; |
} else if (yawTerm > (tmpInt - throttleTerm)) { |
yawTerm = (tmpInt - throttleTerm); |
DebugOut.Digital[0] |= DEBUG_CLIP; |
} |
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
DebugOut.Digital[1] &= ~DEBUG_CLIP; |
for (axis = PITCH; axis <= ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
386,6 → 403,8 |
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. |
} |
|
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
|
// 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. |
397,6 → 416,11 |
* (max. pitch or roll term is the throttle value). |
* TODO: Why a growing function of yaw? |
*/ |
if (term[axis] < -tmp_int) { |
DebugOut.Digital[1] |= DEBUG_CLIP; |
} else if (term[axis] > tmp_int) { |
DebugOut.Digital[1] |= DEBUG_CLIP; |
} |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
// end part 3: 350 - 400 usec. |
421,6 → 445,7 |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
tmp = motorFilters[i] / CONTROL_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, |
428,10 → 453,11 |
// throttle should be limited. |
// --> WRONG. This caused motors to stall completely in tight maneuvers. |
// Apply to individual signals instead. |
CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
CHECK_MIN_MAX(tmp, 1, 255); |
// CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
CHECK_MIN_MAX(tmp, 8, 255); |
motor[i].SetPoint = tmp; |
} else if (motorTestActive) { |
} |
else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
} else { |
motor[i].SetPoint = 0; |