Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1959 → Rev 1960

/branches/dongfang_FC_rewrite/flight.c
61,10 → 61,6
 
// Necessary for external control and motor test
#include "uart0.h"
 
// for scope debugging
// #include "rc.h"
 
#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
86,13 → 82,12
 
// Some integral weight constant...
uint16_t Ki = 10300 / 33;
uint8_t RequiredMotors = 0;
 
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
switch (dynamicParams.UserParams[5]) {
switch (dynamicParams.motorSmoothing) {
case 0:
return newvalue;
case 1:
118,9 → 113,11
void flight_setNeutral() {
MKFlags |= MKFLAG_CALIBRATE;
// not really used here any more.
/*
dynamicParams.KalmanK = -1;
dynamicParams.KalmanMaxDrift = 0;
dynamicParams.KalmanMaxFusion = 32;
*/
controlMixer_initVariables();
}
 
136,10 → 133,10
void setNormalFlightParameters(void) {
setFlightParameters(
dynamicParams.IFactor,
dynamicParams.GyroP,
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
dynamicParams.GyroP,
dynamicParams.UserParams[6]
dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP,
dynamicParams.yawIFactor
);
}
 
179,10 → 176,10
throttleTerm = controls[CONTROL_THROTTLE];
 
// 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);
if (throttleTerm < staticParams.minThrottle + 10)
throttleTerm = staticParams.minThrottle + 10;
else if (throttleTerm > staticParams.maxThrottle - 20)
throttleTerm = (staticParams.maxThrottle - 20);
 
/************************************************************************/
/* RC-signal is bad */
199,8 → 196,8
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
throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
setStableFlightParameters();
} else {
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
207,15 → 204,15
}
} else {
// end emergency flight (just cut the motors???)
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
}
} else {
// signal is acceptable
if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
// The time is in whole seconds.
emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488;
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488;
}
 
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
229,51 → 226,37
* 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;
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);
}
} 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();
 
// 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(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated
if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated
ignoreCompassTimer = 1000;
if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) {
updateCompassCourse = 1;
}
}
 
// yawControlRate = controlYaw;
 
// 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.
285,7 → 268,7
/************************************************************************/
/* Compass is currently not supported. */
/************************************************************************/
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
updateCompass();
}
 
310,21 → 293,16
// 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
}
 
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;
 
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);
}
 
343,7 → 321,7
// 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
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?
381,7 → 359,7
}
 
// FIXME: Throttle may exceed maxThrottle (there is no check no more).
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING;
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
407,7 → 385,7
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.
}
 
tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability
tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
 
// TODO: From which planet comes the 16000?
443,10 → 421,10
int32_t tmp;
uint8_t throttle;
 
tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH];
tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL];
tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW];
tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
tmp = tmp >> 6;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
// Now we scale back down to a 0..255 range.
464,7 → 442,7
 
if (i < 4) debugOut.analog[22 + i] = throttle;
 
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) {
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
motor[i].SetPoint = throttle;
} else if (motorTestActive) {
motor[i].SetPoint = motorTest[i];
486,6 → 464,6
 
debugOut.analog[16] = gyroPFactor;
debugOut.analog[17] = gyroIFactor;
debugOut.analog[18] = dynamicParams.GyroD;
debugOut.analog[18] = dynamicParams.gyroD;
}
}