78,10 → 78,8 |
|
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
uint8_t invKi = 64; |
|
// Some integral weight constant... |
uint16_t Ki = 10300 / 33; |
|
/************************************************************************/ |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
120,9 → 118,9 |
controlMixer_initVariables(); |
} |
|
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor, |
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Ki = 10300 / _Ki; |
invKi = _invKi; |
gyroPFactor = _gyroPFactor; |
gyroIFactor = _gyroIFactor; |
yawPFactor = _yawPFactor; |
140,7 → 138,7 |
} |
|
void setStableFlightParameters(void) { |
setFlightParameters(33, 90, 120, 90, 120); |
setFlightParameters(0, 90, 120, 90, 120); |
} |
|
/************************************************************************/ |
147,12 → 145,12 |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
int16_t tmp_int; |
uint16_t tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, term[2]; |
|
// PID controller variables |
int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
int16_t PDPart; |
static int32_t IPart[2] = {0, 0}; |
static uint16_t emergencyFlightTime; |
static int8_t debugDataTimer = 1; |
216,9 → 214,8 |
* 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; |
PDPartYaw = 0; |
if (isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
if (isFlying == 250) { |
HC_setGround(); |
#ifdef USE_MK3MAG |
228,49 → 225,18 |
// Set target heading to the one just gotten off compass. |
// targetHeading = heading; |
} |
} 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(); |
setNormalFlightParameters(); |
} // end else (not bad signal case) |
|
#if defined (USE_NAVICTRL) |
/************************************************************************/ |
/* GPS is currently not supported. */ |
/************************************************************************/ |
if(staticParams.GlobalConfig & CFG_GPS_ENABLED) { |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} else { |
} |
#endif |
// end part 1: 750-800 usec. |
// start part 3: 350 - 400 usec. |
/************************************************************************/ |
|
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis = PITCH; axis <= ROLL; axis++) { |
PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
|
//CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
} else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
} |
} |
|
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
// This is where control affects the target heading. It also (later) directly controls yaw. |
headingError -= controls[CONTROL_YAW]; |
278,13 → 244,10 |
if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT; |
if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT; |
|
PDPartYaw = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
PDPart = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4); |
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6); |
PDPart += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5); |
|
// limit control feedback |
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
|
/* |
* Compose throttle term. |
* If a Bl-Ctrl is missing, prevent takeoff. |
307,7 → 270,7 |
* between current throttle and maximum throttle). |
*/ |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING; |
// Limit yawTerm |
debugOut.digital[0] &= ~DEBUG_CLIP; |
if (throttleTerm > MIN_YAWGAS) { |
337,37 → 300,38 |
debugOut.digital[0] |= DEBUG_CLIP; |
} |
|
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
debugOut.digital[1] &= ~DEBUG_CLIP; |
|
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6; |
|
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis = PITCH; axis <= ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
*/ |
int16_t iDiff; |
iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
// In HH mode, the I part is summed from P and D of gyros minus stick. |
if (gyroIFactor) { |
// 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] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) 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] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
IPart[axis] += PDPart - 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 |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
|
//CHECK_MIN_MAX(IPart[axis], -25L*GYRO_DEG_FACTOR_PITCHROLL, 25L*GYRO_DEG_FACTOR_PITCHROLL); |
if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) { |
IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL; |
// With normal Ki, limit effect to +/- 205 (of 1024!!!) |
if (IPart[axis] < -64000) { |
IPart[axis] = -64000; |
debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
} else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL; |
} else if (IPart[axis] > 64000) { |
IPart[axis] = 64000; |
debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
} |
|
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14; |
term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |