Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1871 → Rev 1872

/branches/dongfang_FC_rewrite/flight.c
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...
92,22 → 92,23
/* Filter for motor value smoothing (necessary???) */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
switch(dynamicParams.UserParams[5]) {
switch (dynamicParams.UserParams[5]) {
case 0:
return newvalue;
case 1:
return (oldvalue + newvalue) / 2;
return (oldvalue + newvalue) / 2;
case 2:
if(newvalue > oldvalue)
return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new
else
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
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;
default:
return newvalue;
}
}
 
123,7 → 124,8
controlMixer_initVariables();
}
 
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _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;
132,12 → 134,9
}
 
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) {
144,18 → 143,17
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
// 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 IPart[2] = { 0, 0 };
// static int32_t yawControlRate = 0;
 
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
173,13 → 171,14
// 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 = controlThrottle;
 
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);
if (throttleTerm < staticParams.MinThrottle + 10)
throttleTerm = staticParams.MinThrottle + 10;
else if (throttleTerm > staticParams.MaxThrottle - 20)
throttleTerm = (staticParams.MaxThrottle - 20);
 
/************************************************************************/
/* RC-signal is bad */
187,61 → 186,62
/* to the control mixer: An emergency autopilot control. */
/************************************************************************/
 
if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
RED_ON;
beepRCAlarm();
if(emergencyFlightTime) {
 
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();
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.
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 {
} else {
// signal is acceptable
if(controlMixer_getSignalQuality() > SIGNAL_BAD) {
if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
// The time is in whole seconds.
emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488;
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)) {
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 (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();
253,20 → 253,21
* 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;
if (looping) {
if (throttleTerm > staticParams.LoopGasLimit)
throttleTerm = staticParams.LoopGasLimit;
}
 
/************************************************************************/
/* Yawing */
/************************************************************************/
if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
ignoreCompassTimer = 1000;
if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
updateCompassCourse = 1;
}
}
 
// yawControlRate = controlYaw;
 
// Trim drift of yawAngleDiff with controlYaw.
273,17 → 274,17
// 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)) {
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
updateCompass();
}
 
#if defined (USE_NAVICTRL)
/************************************************************************/
/* GPS is currently not supported. */
297,15 → 298,15
// 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).
/* 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)) {
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
316,26 → 317,27
* 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);
}
 
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 (missingMotor) {
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
if(isFlying > 1 && isFlying < 50 && throttleTerm > 0)
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
}
354,22 → 356,22
yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING;
// Limit yawTerm
DebugOut.Digital[0] &= ~DEBUG_CLIP;
if(throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm/2) {
if (throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
yawTerm = -throttleTerm/2;
} else if (yawTerm > throttleTerm/2) {
yawTerm = -throttleTerm / 2;
} else if (yawTerm > throttleTerm / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
yawTerm = throttleTerm/2;
yawTerm = throttleTerm / 2;
}
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
} else {
if (yawTerm < -MIN_YAWGAS/2) {
if (yawTerm < -MIN_YAWGAS / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
yawTerm = -MIN_YAWGAS/2;
} else if (yawTerm > MIN_YAWGAS/2) {
yawTerm = -MIN_YAWGAS / 2;
} else if (yawTerm > MIN_YAWGAS / 2) {
DebugOut.Digital[0] |= DEBUG_CLIP;
yawTerm = MIN_YAWGAS/2;
yawTerm = MIN_YAWGAS / 2;
}
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
386,11 → 388,11
 
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
DebugOut.Digital[1] &= ~DEBUG_CLIP;
for (axis=PITCH; axis<=ROLL; axis++) {
for (axis = PITCH; axis <= ROLL; axis++) {
/*
* Compose pitch and roll terms. This is finally where the sticks come into play.
*/
if(gyroIFactor) {
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.
401,12 → 403,13
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;
 
// 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
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(!).
421,7 → 424,6
}
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
}
// end part 3: 350 - 400 usec.
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
433,13 → 435,14
DebugOut.Analog[14] = yawTerm;
DebugOut.Analog[15] = throttleTerm;
 
for(i = 0; i < MAX_MOTORS; i++) {
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;
tmp = throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
tmp += term[PITCH] * Mixer.Motor[i][MIX_PITCH];
tmp += term[ROLL] * Mixer.Motor[i][MIX_ROLL];
tmp += yawTerm * Mixer.Motor[i][MIX_YAW];
tmp = tmp >> 6;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
// Now we scale back down to a 0..255 range.
tmp = motorFilters[i] / MOTOR_SCALING;
451,36 → 454,36
// throttle should be limited.
// --> WRONG. This caused motors to stall completely in tight maneuvers.
// Apply to individual signals instead.
CHECK_MIN_MAX(tmp, 8, 255);
CHECK_MIN_MAX(tmp, 1, 255);
motor[i].SetPoint = tmp;
}
else if (motorTestActive) {
} else if (motorTestActive) {
motor[i].SetPoint = motorTest[i];
} else {
motor[i].SetPoint = 0;
}
if (i < 4)
DebugOut.Analog[22+i] = motor[i].SetPoint;
DebugOut.Analog[22 + i] = motor[i].SetPoint;
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!(--debugDataTimer)) {
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[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];
*/
}
}