Subversion Repositories FlightCtrl

  int16_t tmp_int;

  int16_t tmp_int;

  // Mixer Fractions that are combined for Motor Control

  // Mixer Fractions that are combined for Motor Control

  int16_t yawTerm, throttleTerm, term[2];

  int16_t yawTerm, throttleTerm, term[2];

  int16_t PPArt[2], DPart[2], PPartYaw, DPartYaw;

  int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */;

  static int32_t IPart[2] = { 0, 0 };

  static int32_t IPart[2] = { 0, 0 };

  static uint16_t emergencyFlightTime;

  static uint16_t emergencyFlightTime;

  static int8_t debugDataTimer = 1;

  static int8_t debugDataTimer = 1;

  /* Calculate control feedback from angle (gyro integral)                */

  /* Calculate control feedback from angle (gyro integral)                */

  /* and angular velocity (gyro signal)                                   */

  /* and angular velocity (gyro signal)                                   */

  /************************************************************************/

  /************************************************************************/

  // The P-part is the P of the PID controller. That's the angle integrals (not rates).

  // The P-part is the P of the PID controller. That's the angle integrals (not rates).

  for (axis = PITCH; axis <= ROLL; axis++) {

  for (axis = PITCH; axis <= ROLL; axis++) {

    /*

    /*

     * Now blend in the D-part - proportional to the Differential of the integral = the rate.

     * 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

     * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING

     * where pfactor is in [0..1].

     * where pfactor is in [0..1].

    DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;

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

    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);

  PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));

  PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));

  DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING);

  PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING);

     */

     */

    if (gyroIFactor) {

    if (gyroIFactor) {

      // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.

      // 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.

      // 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.

      // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.

    } else {

    } else {

      // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.

      // "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...

      // 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[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.

    }

    }