| 81,7 → 81,7 |
|---|
| */ |
| // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| |
| uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
| |
| // Some integral weight constant... |
| 95,24 → 95,23 |
|---|
| /* Filter for motor value smoothing (necessary???) */ |
| /************************************************************************/ |
| int16_t motorFilter(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; |
| } |
| 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; |
| } |
| } |
| |
| /************************************************************************/ |
| 119,345 → 118,372 |
|---|
| /* Neutral Readings */ |
| /************************************************************************/ |
| void flight_setNeutral() { |
| MKFlags |= MKFLAG_CALIBRATE; |
| MKFlags |= MKFLAG_CALIBRATE; |
| |
| // not really used here any more. |
| dynamicParams.KalmanK = -1; |
| dynamicParams.KalmanMaxDrift = 0; |
| dynamicParams.KalmanMaxFusion = 32; |
| // not really used here any more. |
| dynamicParams.KalmanK = -1; |
| dynamicParams.KalmanMaxDrift = 0; |
| dynamicParams.KalmanMaxFusion = 32; |
| |
| controlMixer_initVariables(); |
| controlMixer_initVariables(); |
| } |
| |
| void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
| uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| Ki = 10300 / _Ki; |
| gyroPFactor = _gyroPFactor; |
| gyroIFactor = _gyroIFactor; |
| yawPFactor = _yawPFactor; |
| yawIFactor = _yawIFactor; |
| void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| Ki = 10300 / _Ki; |
| gyroPFactor = _gyroPFactor; |
| gyroIFactor = _gyroIFactor; |
| yawPFactor = _yawPFactor; |
| yawIFactor = _yawIFactor; |
| } |
| |
| void setNormalFlightParameters(void) { |
| setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10, |
| staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
| dynamicParams.GyroP + 10, dynamicParams.UserParams[6]); |
| setFlightParameters(dynamicParams.IFactor + 1, |
| dynamicParams.GyroP + 10, |
| staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
| dynamicParams.GyroP + 10, |
| dynamicParams.UserParams[6] |
| ); |
| } |
| |
| void setStableFlightParameters(void) { |
| setFlightParameters(33, 90, 120, 90, 120); |
| setFlightParameters(33, 90, 120, 90, 120); |
| } |
| |
| |
| /************************************************************************/ |
| /* Main Flight Control */ |
| /************************************************************************/ |
| void flight_control(void) { |
| int16_t tmp_int; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t yawTerm, throttleTerm, term[2]; |
| int16_t tmp_int; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t yawTerm, throttleTerm, term[2]; |
| |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw, PPart[2]; |
| static int32_t IPart[2] = { 0, 0 }; |
| // static int32_t yawControlRate = 0; |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw, PPart[2]; |
| static int32_t IPart[2] = {0,0}; |
| // static int32_t yawControlRate = 0; |
| |
| // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
| // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
| // static int32_t CorrectionPitch, CorrectionRoll; |
| // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
| // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
| // static int32_t CorrectionPitch, CorrectionRoll; |
| |
| static uint16_t emergencyFlightTime; |
| static int8_t debugDataTimer = 1; |
| static uint16_t emergencyFlightTime; |
| static int8_t debugDataTimer = 1; |
| |
| // High resolution motor values for smoothing of PID motor outputs |
| static int16_t motorFilters[MAX_MOTORS]; |
| // High resolution motor values for smoothing of PID motor outputs |
| static int16_t motorFilters[MAX_MOTORS]; |
| |
| uint8_t i, axis; |
| uint8_t i, axis; |
| |
| controlMixer_update(); |
| controlMixer_update(); |
| |
| // Fire the main flight attitude calculation, including integration of angles. |
| calculateFlightAttitude(); |
| // Fire the main flight attitude calculation, including integration of angles. |
| calculateFlightAttitude(); |
| |
| throttleTerm = controlThrottle; |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| // if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| // else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
| throttleTerm = controlThrottle; |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
| |
| /************************************************************************/ |
| /* RC-signal is bad */ |
| /* This part could be abstracted, as having yet another control input */ |
| /* to the control mixer: An emergency autopilot control. */ |
| /************************************************************************/ |
| /************************************************************************/ |
| /* RC-signal is bad */ |
| /* This part could be abstracted, as having yet another control input */ |
| /* to the control mixer: An emergency autopilot control. */ |
| /************************************************************************/ |
| |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| RED_ON; |
| beepRCAlarm(); |
| if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| RED_ON; |
| beepRCAlarm(); |
| |
| if(emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| if(isFlying > 256) { |
| // We're probably still flying. Descend slowly. |
| throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
| MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
| setStableFlightParameters(); |
| } else { |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
| } |
| } else { |
| // end emergency flight (just cut the motors???) |
| MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
| } |
| } else { |
| // signal is acceptable |
| if(controlMixer_getSignalQuality() > SIGNAL_BAD) { |
| // Reset emergency landing control variables. |
| MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
| // The time is in whole seconds. |
| emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488; |
| } |
| |
| if (emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| if (isFlying > 256) { |
| // We're probably still flying. Descend slowly. |
| throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
| MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
| setStableFlightParameters(); |
| } else { |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
| } |
| } else { |
| // end emergency flight (just cut the motors???) |
| MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
| } |
| } else { |
| // signal is acceptable |
| if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
| // Reset emergency landing control variables. |
| MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
| // The time is in whole seconds. |
| emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
| } |
| // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
| if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| // increment flight-time counter until overflow. |
| if(isFlying != 0xFFFF) isFlying++; |
| } else |
| /* |
| * When standing on the ground, do not apply I controls and zero the yaw stick. |
| * 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; |
| // TODO: Don't stomp on other modules' variables!!! |
| // controlYaw = 0; |
| PDPartYaw = 0; // instead. |
| if(isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| yawAngleDiff = 0; |
| } |
| } 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); |
| } |
| |
| // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
| if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| // increment flight-time counter until overflow. |
| if (isFlying != 0xFFFF) |
| isFlying++; |
| } else |
| /* |
| * When standing on the ground, do not apply I controls and zero the yaw stick. |
| * 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; |
| // TODO: Don't stomp on other modules' variables!!! |
| // controlYaw = 0; |
| PDPartYaw = 0; // instead. |
| if (isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| yawAngleDiff = 0; |
| } |
| } 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(); |
| |
| commands_handleCommands(); |
| // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| setNormalFlightParameters(); |
| // } |
| } // end else (not bad signal case) |
| // end part1a: 750-800 usec. |
| /* |
| * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
| * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
| * This is the throttle part. |
| */ |
| if(looping) { |
| if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
| } |
| |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| } |
| } |
| |
| // yawControlRate = controlYaw; |
| |
| // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| setNormalFlightParameters(); |
| // } |
| } // end else (not bad signal case) |
| // end part1a: 750-800 usec. |
| /* |
| * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
| * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
| * This is the throttle part. |
| */ |
| if (looping) { |
| if (throttleTerm > staticParams.LoopGasLimit) |
| throttleTerm = staticParams.LoopGasLimit; |
| } |
| |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| } |
| } |
| |
| // yawControlRate = controlYaw; |
| |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| |
| #if defined (USE_NAVICTRL) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } else { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| } |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } else { |
| } |
| #endif |
| // end part 1: 750-800 usec. |
| // start part 3: 350 - 400 usec. |
| // end part 1: 750-800 usec. |
| // start part 3: 350 - 400 usec. |
| #define SENSOR_LIMIT (4096 * 4) |
| /************************************************************************/ |
| /************************************************************************/ |
| |
| /* 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++) { |
| if (looping & ((1 << 4) << axis)) { |
| PPart[axis] = 0; |
| } else { // TODO: Where do the 44000 come from??? |
| PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| } |
| /* 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++) { |
| if(looping & ((1<<4)<<axis)) { |
| PPart[axis] = 0; |
| } else { // TODO: Where do the 44000 come from??? |
| PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| } |
| |
| /* |
| * 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 |
| * where pfactor is in [0..1]. |
| */ |
| PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
| * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
| * (int16_t) dynamicParams.GyroD) / 16; |
| /* |
| * 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 |
| * where pfactor is in [0..1]. |
| */ |
| PDPart[axis] = PPart[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); |
| } |
| |
| PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
| / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
| / CONTROL_SCALING)); |
| PDPartYaw = |
| (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
| + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| |
| // limit control feedback |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| |
| /* |
| * Compose throttle term. |
| * If a Bl-Ctrl is missing, prevent takeoff. |
| */ |
| if(missingMotor) { |
| // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| if(isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
| isFlying = 1; // keep within lift off condition |
| throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
| } |
| |
| // limit control feedback |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
| throttleTerm *= CONTROL_SCALING; |
| |
| /* |
| * Compose throttle term. |
| * If a Bl-Ctrl is missing, prevent takeoff. |
| */ |
| if (missingMotor) { |
| // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
| isFlying = 1; // keep within lift off condition |
| throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
| } |
| |
| // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
| throttleTerm *= CONTROL_SCALING; |
| |
| /* |
| * Compose yaw term. |
| * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
| * However, at low throttle the yaw term is limited to a fixed value, |
| * and at high throttle it is limited by the throttle reserve (the difference |
| * between current throttle and maximum throttle). |
| */ |
| /* |
| * Compose yaw term. |
| * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
| * However, at low throttle the yaw term is limited to a fixed value, |
| * and at high throttle it is limited by the throttle reserve (the difference |
| * between current throttle and maximum throttle). |
| */ |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| // Limit yawTerm |
| if (throttleTerm > MIN_YAWGAS) { |
| CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
| } else { |
| CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| // Limit yawTerm |
| DebugOut.Digital[0] &= ~DEBUG_CLIP; |
| if(throttleTerm > MIN_YAWGAS) { |
| if (yawTerm < -throttleTerm/2) { |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| yawTerm = -throttleTerm/2; |
| } else if (yawTerm > throttleTerm/2) { |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| yawTerm = throttleTerm/2; |
| } |
| //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
| } else { |
| if (yawTerm < -MIN_YAWGAS/2) { |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| yawTerm = -MIN_YAWGAS/2; |
| } else if (yawTerm > MIN_YAWGAS/2) { |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| yawTerm = MIN_YAWGAS/2; |
| } |
| //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| |
| tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
| CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
| // FIXME: Throttle may exceed maxThrottle (there is no check no more). |
| tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
| if (yawTerm < -(tmpInt - throttleTerm)) { |
| yawTerm = -(tmpInt - throttleTerm); |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| } else if (yawTerm > (tmpInt - throttleTerm)) { |
| yawTerm = (tmpInt - throttleTerm); |
| DebugOut.Digital[0] |= DEBUG_CLIP; |
| } |
| // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
| DebugOut.Digital[1] &= ~DEBUG_CLIP; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| /* |
| * Compose pitch and roll terms. This is finally where the sticks come into play. |
| */ |
| 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] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) 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] - control[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; |
| tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
| |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| /* |
| * Compose pitch and roll terms. This is finally where the sticks come into play. |
| */ |
| 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] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) 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] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| } |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
| * (max. pitch or roll term is the throttle value). |
| * TODO: Why a growing function of yaw? |
| */ |
| if (term[axis] < -tmp_int) { |
| DebugOut.Digital[1] |= DEBUG_CLIP; |
| } else if (term[axis] > tmp_int) { |
| DebugOut.Digital[1] |= DEBUG_CLIP; |
| } |
| CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
| } |
| // end part 3: 350 - 400 usec. |
| |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
| * (max. pitch or roll term is the throttle value). |
| * TODO: Why a growing function of yaw? |
| */ |
| CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
| } |
| // end part 3: 350 - 400 usec. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |
| // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // 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] = yawTerm; |
| DebugOut.Analog[15] = throttleTerm; |
| |
| DebugOut.Analog[12] = term[PITCH]; |
| DebugOut.Analog[13] = term[ROLL]; |
| DebugOut.Analog[14] = yawTerm; |
| DebugOut.Analog[15] = throttleTerm; |
| for(i = 0; i < MAX_MOTORS; i++) { |
| int16_t tmp; |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
| tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
| tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
| tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / CONTROL_SCALING; |
| |
| for (i = 0; i < MAX_MOTORS; i++) { |
| int16_t tmp; |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
| tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
| tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
| tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / CONTROL_SCALING; |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
| CHECK_MIN_MAX(tmp, 1, 255); |
| motor[i].SetPoint = tmp; |
| } else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| } |
| if (i < 4) |
| DebugOut.Analog[22 + i] = motor[i].SetPoint; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| // CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
| CHECK_MIN_MAX(tmp, 8, 255); |
| motor[i].SetPoint = tmp; |
| } |
| else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| } |
| if (i < 4) |
| DebugOut.Analog[22+i] = motor[i].SetPoint; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // 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; |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // 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[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
| DebugOut.Analog[24] = controlYaw; |
| DebugOut.Analog[25] = yawAngleDiff / 100L; |
| DebugOut.Analog[26] = accNoisePeak[PITCH]; |
| DebugOut.Analog[27] = accNoisePeak[ROLL]; |
| DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| */ |
| } |
| /* |
| DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
| DebugOut.Analog[24] = controlYaw; |
| DebugOut.Analog[25] = yawAngleDiff / 100L; |
| DebugOut.Analog[26] = accNoisePeak[PITCH]; |
| DebugOut.Analog[27] = accNoisePeak[ROLL]; |
| DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| */ |
| } |
| } |