| 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(!). |