| 7,9 → 7,6 |
|---|
| // Necessary for external control and motor test |
| #include "uart0.h" |
| |
| // for scope debugging |
| // #include "rc.h" |
| |
| #include "timer2.h" |
| #include "attitude.h" |
| #include "controlMixer.h" |
| 32,7 → 29,7 |
|---|
| uint8_t reverse[3]; |
| int32_t IPart[3] = { 0, 0, 0 }; |
| |
| int16_t servos[MAX_SERVOS]; |
| int16_t controlServos[MAX_CONTROL_SERVOS]; |
| |
| /************************************************************************/ |
| /* Neutral Readings */ |
| 40,41 → 37,38 |
|---|
| #define CONTROL_CONFIG_SCALE 10 |
| |
| void flight_setGround(void) { |
| IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
| target[PITCH] = attitude[PITCH]; |
| target[ROLL] = attitude[ROLL]; |
| target[YAW] = attitude[YAW]; |
| IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
| target[PITCH] = attitude[PITCH]; |
| target[ROLL] = attitude[ROLL]; |
| target[YAW] = attitude[YAW]; |
| } |
| |
| void switchToFlightMode(FlightMode_t flightMode) { |
| if (flightMode == FLIGHT_MODE_MANUAL) { |
| pFactor[PITCH] = 0; |
| pFactor[ROLL] = 0; |
| pFactor[YAW] = 0; |
| } else if (flightMode == FLIGHT_MODE_RATE) { |
| pFactor[PITCH] = 0; //staticParams...; |
| pFactor[ROLL] = 0; //staticParams...; |
| pFactor[YAW] = 0; //staticParams...; |
| } |
| if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
| iFactor[PITCH] = 0; |
| iFactor[ROLL] = 0; |
| iFactor[YAW] = 0; |
| } else if (flightMode == FLIGHT_MODE_ANGLES) { |
| iFactor[PITCH] = 0; //staticParams...; |
| iFactor[ROLL] = 0; //staticParams...; |
| iFactor[YAW] = 0; //staticParams...; |
| // When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
| target[PITCH] = attitude[PITCH]; |
| target[ROLL] = attitude[ROLL]; |
| target[YAW] = attitude[YAW]; |
| } |
| // this should be followed by a call to switchToFlightMode!! |
| void updateFlightParametersToFlightMode(uint8_t flightMode) { |
| debugOut.analog[15] = flightMode; |
| |
| dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
| dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
| dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
| reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[CONTROL_AILERONS] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[CONTROL_RUDDER] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_RUDDER; |
| |
| // TODO: Set reverse also. |
| for (uint8_t i = 0; i < 3; i++) { |
| if (flightMode == FLIGHT_MODE_MANUAL) { |
| pFactor[i] = 0; |
| dFactor[i] = 0; |
| } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
| pFactor[i] = staticParams.gyroPID[i].P; |
| dFactor[i] = staticParams.gyroPID[i].D; |
| } |
| |
| if (flightMode == FLIGHT_MODE_ANGLES) { |
| iFactor[i] = staticParams.gyroPID[i].I; |
| } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
| iFactor[i] = 0; |
| } |
| } |
| } |
| |
| /************************************************************************/ |
| 81,120 → 75,130 |
|---|
| /* Main Flight Control */ |
| /************************************************************************/ |
| void flight_control(void) { |
| // Mixer Fractions that are combined for Motor Control |
| int16_t throttleTerm, term[3]; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t throttleTerm, term[3]; |
| |
| // PID controller variables |
| int16_t PDPart[3]; |
| // PID controller variables |
| int16_t PDPart[3]; |
| |
| static int8_t debugDataTimer = 1; |
| static int8_t debugDataTimer = 1; |
| |
| // High resolution motor values for smoothing of PID motor outputs |
| // static int16_t outputFilters[MAX_OUTPUTS]; |
| // High resolution motor values for smoothing of PID motor outputs |
| // static int16_t outputFilters[MAX_OUTPUTS]; |
| |
| uint8_t axis; |
| uint8_t axis; |
| |
| // TODO: Check modern version. |
| // calculateFlightAttitude(); |
| // TODO: Check modern version. |
| // controlMixer_update(); |
| throttleTerm = controls[CONTROL_THROTTLE]; |
| // TODO: Check modern version. |
| // calculateFlightAttitude(); |
| // TODO: Check modern version. |
| // controlMixer_update(); |
| throttleTerm = controls[CONTROL_THROTTLE]; |
| |
| // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
| target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
| target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
| // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
| target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
| target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
| |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| target[axis] -= OVER360; |
| } else if (attitude[axis] <= -OVER180) { |
| attitude[axis] += OVER360; |
| } |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| target[axis] -= OVER360; |
| } else if (attitude[axis] <= -OVER180) { |
| attitude[axis] += OVER360; |
| } |
| |
| if (reverse[axis]) |
| error[axis] = attitude[axis] + target[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| if (error[axis] > OVER180) { |
| error[axis] -= OVER360; |
| } else if (error[axis] <= -OVER180) { |
| error[axis] += OVER360; |
| } |
| if (reverse[axis]) |
| error[axis] = attitude[axis] + target[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| if (error[axis] > OVER180) { |
| error[axis] -= OVER360; |
| } else if (error[axis] <= -OVER180) { |
| error[axis] += OVER360; |
| } |
| |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
| + ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
| + ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| |
| int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| |
| // Add I parts here... these are integrated errors. |
| // When an error wraps, actually its I part should be negated or something... |
| // Add I parts here... these are integrated errors. |
| // When an error wraps, actually its I part should be negated or something... |
| |
| term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
| } |
| term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
| } |
| |
| debugOut.analog[12] = term[PITCH]; |
| debugOut.analog[13] = term[ROLL]; |
| debugOut.analog[14] = throttleTerm; |
| debugOut.analog[15] = term[YAW]; |
| debugOut.analog[12] = term[PITCH]; |
| debugOut.analog[13] = term[ROLL]; |
| debugOut.analog[14] = throttleTerm; |
| debugOut.analog[15] = term[YAW]; |
| |
| for (uint8_t i = 0; i < MAX_SERVOS; i++) { |
| int16_t tmp; |
| if (servoTestActive) { |
| servos[i] = ((int16_t) servoTest[i] - 128) * 4; |
| } else { |
| // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
| switch (i) { |
| case 0: |
| tmp = term[ROLL]; |
| break; |
| case 1: |
| tmp = term[PITCH]; |
| break; |
| case 2: |
| tmp = throttleTerm; |
| break; |
| case 3: |
| tmp = term[YAW]; |
| break; |
| default: |
| tmp = 0; |
| } |
| // These are all signed and in the same units as the RC stuff in rc.c. |
| servos[i] = tmp; |
| } |
| } |
| for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) { |
| int16_t tmp; |
| if (servoTestActive) { |
| controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
| } else { |
| // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
| switch (i) { |
| case 0: |
| tmp = term[ROLL]; |
| break; |
| case 1: |
| tmp = term[PITCH]; |
| break; |
| case 2: |
| tmp = throttleTerm; |
| break; |
| case 3: |
| tmp = term[YAW]; |
| break; |
| default: |
| tmp = 0; |
| } |
| // These are all signed and in the same units as the RC stuff in rc.c. |
| controlServos[i] = tmp; |
| } |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if (!(--debugDataTimer)) { |
| debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
| calculateControlServoValues; |
| |
| debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if (!(--debugDataTimer)) { |
| debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| debugOut.analog[0] = rate_PID[PITCH]; // in 0.1 deg |
| debugOut.analog[1] = rate_PID[ROLL]; // in 0.1 deg |
| debugOut.analog[2] = rate_PID[YAW]; |
| |
| debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
| debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
| |
| //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
| //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
| } |
| debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
| |
| debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
| |
| debugOut.analog[12] = term[PITCH]; |
| debugOut.analog[13] = term[ROLL]; |
| debugOut.analog[14] = term[YAW]; |
| |
| //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
| //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
| } |
| } |