61,10 → 61,6 |
|
// Necessary for external control and motor test |
#include "uart0.h" |
|
// for scope debugging |
// #include "rc.h" |
|
#include "twimaster.h" |
#include "attitude.h" |
#include "controlMixer.h" |
86,13 → 82,12 |
|
// Some integral weight constant... |
uint16_t Ki = 10300 / 33; |
uint8_t RequiredMotors = 0; |
|
/************************************************************************/ |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
switch (dynamicParams.UserParams[5]) { |
switch (dynamicParams.motorSmoothing) { |
case 0: |
return newvalue; |
case 1: |
118,9 → 113,11 |
void flight_setNeutral() { |
MKFlags |= MKFLAG_CALIBRATE; |
// not really used here any more. |
/* |
dynamicParams.KalmanK = -1; |
dynamicParams.KalmanMaxDrift = 0; |
dynamicParams.KalmanMaxFusion = 32; |
*/ |
controlMixer_initVariables(); |
} |
|
136,10 → 133,10 |
void setNormalFlightParameters(void) { |
setFlightParameters( |
dynamicParams.IFactor, |
dynamicParams.GyroP, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP, |
dynamicParams.UserParams[6] |
dynamicParams.gyroP, |
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
dynamicParams.gyroP, |
dynamicParams.yawIFactor |
); |
} |
|
179,10 → 176,10 |
throttleTerm = controls[CONTROL_THROTTLE]; |
|
// 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 */ |
199,8 → 196,8 |
emergencyFlightTime--; |
if (isFlying > 256) { |
// We're probably still flying. Descend slowly. |
throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing |
setStableFlightParameters(); |
} else { |
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
207,15 → 204,15 |
} |
} else { |
// end emergency flight (just cut the motors???) |
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT); |
} |
} else { |
// signal is acceptable |
if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
// Reset emergency landing control variables. |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
// The time is in whole seconds. |
emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
} |
|
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
229,51 → 226,37 |
* Probably to avoid integration effects that will cause the copter to spin |
* or flip when taking off. |
*/ |
if (isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
// controlYaw = 0; |
PDPartYaw = 0; // instead. |
if (isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
yawAngleDiff = 0; |
if (isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
// controlYaw = 0; |
PDPartYaw = 0; // instead. |
if (isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
yawAngleDiff = 0; |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
MKFlags |= (MKFLAG_FLY); |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
MKFlags |= (MKFLAG_FLY); |
} |
|
|
commands_handleCommands(); |
|
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
setNormalFlightParameters(); |
// } |
} // end else (not bad signal case) |
// end part1a: 750-800 usec. |
/* |
* Looping the H&I way basically is just a matter of turning off attitude angle measurement |
* by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
* This is the throttle part. |
*/ |
if (looping) { |
if (throttleTerm > staticParams.LoopGasLimit) |
throttleTerm = staticParams.LoopGasLimit; |
} |
|
|
/************************************************************************/ |
/* Yawing */ |
/************************************************************************/ |
if (abs(controls[CONTROL_YAW]) > 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)) { |
if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
updateCompassCourse = 1; |
} |
} |
|
// yawControlRate = controlYaw; |
|
// yawControlRate = controlYaw; |
// 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. |
285,7 → 268,7 |
/************************************************************************/ |
/* Compass is currently not supported. */ |
/************************************************************************/ |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
updateCompass(); |
} |
|
310,21 → 293,16 |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
|
for (axis = PITCH; axis <= ROLL; axis++) { |
if (looping & ((1 << 4) << axis)) { |
PPart[axis] = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
} |
|
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
|
/* |
* Now blend in the D-part - proportional to the Differential of the integral = the rate. |
* 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); |
} |
|
343,7 → 321,7 |
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
isFlying = 1; // keep within lift off condition |
throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of |
} |
|
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
381,7 → 359,7 |
} |
|
// FIXME: Throttle may exceed maxThrottle (there is no check no more). |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
tmp_int = staticParams.maxThrottle * CONTROL_SCALING; |
if (yawTerm < -(tmp_int - throttleTerm)) { |
yawTerm = -(tmp_int - throttleTerm); |
debugOut.digital[0] |= DEBUG_CLIP; |
407,7 → 385,7 |
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 |
tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
|
// TODO: From which planet comes the 16000? |
443,10 → 421,10 |
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 = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE]; |
tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH]; |
tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL]; |
tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
464,7 → 442,7 |
|
if (i < 4) debugOut.analog[22 + i] = throttle; |
|
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) { |
motor[i].SetPoint = throttle; |
} else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
486,6 → 464,6 |
|
debugOut.analog[16] = gyroPFactor; |
debugOut.analog[17] = gyroIFactor; |
debugOut.analog[18] = dynamicParams.GyroD; |
debugOut.analog[18] = dynamicParams.gyroD; |
} |
} |