| 54,14 → 54,20 |
|---|
| #include "eeprom.h" |
| #include "flight.h" |
| #include "output.h" |
| #include "uart0.h" |
| |
| // Necessary for external control and motor test |
| #include "uart0.h" |
| #include "twimaster.h" |
| #include "attitude.h" |
| #include "controlMixer.h" |
| #include "commands.h" |
| #include "heightControl.h" |
| |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #include "compassControl.h" |
| #endif |
| |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| |
| /* |
| 147,7 → 153,7 |
|---|
| |
| // PID controller variables |
| 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 int8_t debugDataTimer = 1; |
| |
| 171,7 → 177,7 |
|---|
| /************************************************************************/ |
| |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| if (controlMixer_didReceiveSignal) beepRCAlarm(); |
| if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
| if (emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| 193,7 → 199,10 |
|---|
| // Reset emergency landing control variables. |
| MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
| // The time is in whole seconds. |
| emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
| if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP) |
| emergencyFlightTime = 0xffff; |
| else |
| emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP; |
| } |
| |
| // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
| 211,8 → 220,11 |
|---|
| IPart[PITCH] = IPart[ROLL] = 0; |
| PDPartYaw = 0; |
| if (isFlying == 250) { |
| // HC_setGround(); |
| HC_setGround(); |
| #ifdef USE_MK3MAG |
| attitude_resetHeadingToMagnetic(); |
| compass_setTakeoffHeading(heading); |
| #endif |
| // Set target heading to the one just gotten off compass. |
| // targetHeading = heading; |
| } |
| 226,24 → 238,11 |
|---|
| setNormalFlightParameters(); |
| } // end else (not bad signal case) |
| |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| /* |
| if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
| //targetHeading = heading; |
| // YGBSM!!! |
| } |
| } |
| */ |
| |
| #if defined (USE_NAVICTRL) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| if(staticParams.GlobalConfig & CFG_GPS_ENABLED) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } else { |
| 261,7 → 260,15 |
|---|
| 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); |
| |
| //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) |
| 350,8 → 357,15 |
|---|
| tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
| * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
| |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| //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; |
| debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
| } else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) { |
| PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL; |
| 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] += (dynamicParams.levelCorrection[axis] - 128); |