| 0,0 → 1,226 |
|---|
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include "eeprom.h" |
| #include "flight.h" |
| #include "output.h" |
| |
| // 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" |
| #include "commands.h" |
| #ifdef USE_MK3MAG |
| #include "gps.h" |
| #endif |
| |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| |
| /* |
| * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
| * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
| */ |
| // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| |
| int8_t pitchPFactor, rollPFactor, yawPFactor; |
| int8_t pitchDFactor, rollDFactor, yawDFactor; |
| |
| int32_t IPart[2] = {0,0}; |
| |
| /************************************************************************/ |
| /* Filter for motor value smoothing (necessary???) */ |
| /************************************************************************/ |
| int16_t outputFilter(int16_t newvalue, int16_t oldvalue) { |
| switch (dynamicParams.UserParams[5]) { |
| case 0: |
| return newvalue; |
| case 1: |
| return (oldvalue + newvalue) / 2; |
| case 2: |
| if (newvalue > oldvalue) |
| return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
| else |
| return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
| case 3: |
| if (newvalue < oldvalue) |
| return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
| else |
| return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
| default: |
| return newvalue; |
| } |
| } |
| |
| /************************************************************************/ |
| /* Neutral Readings */ |
| /************************************************************************/ |
| #define CONTROL_CONFIG_SCALE 10 |
| |
| void flight_setNeutral() { |
| MKFlags |= MKFLAG_CALIBRATE; |
| // not really used here any more. |
| controlMixer_initVariables(); |
| } |
| |
| void setFlightParameters |
| ( |
| uint8_t _pitchPFactor, |
| uint8_t _rollPFactor, |
| uint8_t _yawPFactor, |
| |
| uint8_t _pitchDFactor, |
| uint8_t _rollDFactor, |
| uint8_t _yawDFactor |
| ) { |
| pitchPFactor = _pitchPFactor; |
| rollPFactor = _rollPFactor; |
| yawPFactor = _yawPFactor; |
| |
| pitchDFactor = _pitchDFactor; |
| rollDFactor = _rollDFactor; |
| yawDFactor = _yawDFactor; |
| } |
| |
| void setNormalFlightParameters(void) { |
| setFlightParameters |
| ( |
| dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good |
| dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good |
| dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little |
| |
| dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE, |
| dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE, |
| dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE |
| ); |
| } |
| |
| void setStableFlightParameters(void) { |
| setFlightParameters(0, 0, 0, 0, 0, 0); |
| } |
| |
| /************************************************************************/ |
| /* Main Flight Control */ |
| /************************************************************************/ |
| void flight_control(void) { |
| // Mixer Fractions that are combined for Motor Control |
| int16_t yawTerm, throttleTerm, term[2]; |
| |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw; |
| |
| static int8_t debugDataTimer = 1; |
| |
| // High resolution motor values for smoothing of PID motor outputs |
| static int16_t outputFilters[MAX_OUTPUTS]; |
| |
| uint8_t i; |
| |
| // Fire the main flight attitude calculation, including integration of angles. |
| // We want that to kick as early as possible, not to delay new AD sampling further. |
| calculateFlightAttitude(); |
| controlMixer_update(); |
| throttleTerm = control[CONTROL_THROTTLE]; |
| |
| /************************************************************************/ |
| /* RC-signal is bad */ |
| /************************************************************************/ |
| |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy |
| RED_ON; |
| beepRCAlarm(); |
| setStableFlightParameters(); |
| } else { |
| commands_handleCommands(); |
| setNormalFlightParameters(); |
| } |
| |
| /************************************************************************/ |
| |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
| (256L / CONTROL_SCALING)) |
| + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16; |
| |
| PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor / |
| (256L / CONTROL_SCALING)) |
| + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
| |
| PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
| + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
| |
| |
| IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
| if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
| else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |
| if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE; |
| else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE; |
| |
| IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL]; |
| if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360; |
| else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360; |
| if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE; |
| else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE; |
| |
| term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]); |
| term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]); |
| yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw); |
| |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |
| // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| DebugOut.Analog[12] = term[PITCH]; |
| DebugOut.Analog[13] = term[ROLL]; |
| DebugOut.Analog[14] = throttleTerm; |
| DebugOut.Analog[15] = yawTerm; |
| |
| for (i = 0; i < MAX_OUTPUTS; i++) { |
| int16_t tmp; |
| if (outputTestActive) { |
| outputs[i].SetPoint = outputTest[i] * 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 - 310; break; |
| case 3: tmp = yawTerm; break; |
| default: tmp = 0; |
| } |
| outputFilters[i] = outputFilter(tmp, outputFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = outputFilters[i]; |
| outputs[i].SetPoint = tmp; |
| } |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if (!(--debugDataTimer)) { |
| debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| |
| DebugOut.Analog[6] = pitchPFactor; |
| DebugOut.Analog[7] = rollPFactor; |
| DebugOut.Analog[8] = yawPFactor; |
| DebugOut.Analog[9] = pitchDFactor; |
| DebugOut.Analog[10] = rollDFactor; |
| DebugOut.Analog[11] = yawDFactor; |
| |
| 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_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| } |
| } |