Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2054 → Rev 2055

/branches/dongfang_FC_rewrite/flight.c
1,54 → 1,3
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// + for non-commercial use (directly or indirectly)
// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
75,10 → 24,10
* 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;
 
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
uint8_t invKi = 64;
uint8_t invKi;
int32_t IPart[2];
 
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
104,21 → 53,7
}
}
 
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
void flight_setNeutral() {
MKFlags |= MKFLAG_CALIBRATE;
// not really used here any more.
/*
dynamicParams.KalmanK = -1;
dynamicParams.KalmanMaxDrift = 0;
dynamicParams.KalmanMaxFusion = 32;
*/
controlMixer_initVariables();
}
 
void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor,
void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor,
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
invKi = _invKi;
gyroPFactor = _gyroPFactor;
127,18 → 62,18
yawIFactor = _yawIFactor;
}
 
void setNormalFlightParameters(void) {
setFlightParameters(
staticParams.IFactor,
dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP,
staticParams.yawIFactor
);
void flight_setGround() {
// Just reset all I terms.
IPart[PITCH] = IPart[ROLL] = 0;
headingError = 0;
}
 
void setStableFlightParameters(void) {
setFlightParameters(0, 90, 120, 90, 120);
void flight_takeOff() {
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
compass_setTakeoffHeading(heading);
#endif
}
 
/************************************************************************/
145,14 → 80,12
/* Main Flight Control */
/************************************************************************/
void flight_control(void) {
uint16_t tmp_int;
int16_t tmp_int;
// Mixer Fractions that are combined for Motor Control
int16_t yawTerm, throttleTerm, term[2];
 
// PID controller variables
int16_t PDPart;
static int32_t IPart[2] = {0, 0};
static uint16_t emergencyFlightTime;
static int8_t debugDataTimer = 1;
 
// High resolution motor values for smoothing of PID motor outputs
162,6 → 95,22
 
throttleTerm = controls[CONTROL_THROTTLE];
 
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
// increment flight-time counter until overflow.
if (isFlying != 0xFFFF)
isFlying++;
}
/*
* 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) {
flight_setGround();
if (isFlying == 250)
flight_takeOff();
}
 
// This check removed. Is done on a per-motor basis, after output matrix multiplication.
if (throttleTerm < staticParams.minThrottle + 10)
throttleTerm = staticParams.minThrottle + 10;
168,100 → 117,27
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. */
/************************************************************************/
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
// TODO: We dont need to repeat this for every iteration!
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
if (emergencyFlightTime) {
// continue emergency flight
emergencyFlightTime--;
if (isFlying > 256) {
// We're probably still flying. Descend slowly.
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.
}
} else {
// end emergency flight (just cut the motors???)
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
}
} else {
// signal is acceptable
if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
// The time is in whole seconds.
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.
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;
if (isFlying == 250) {
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
compass_setTakeoffHeading(heading);
#endif
// Set target heading to the one just gotten off compass.
// targetHeading = heading;
}
} 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();
setNormalFlightParameters();
} // end else (not bad signal case)
// 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 YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
// This is where control affects the target heading. It also (later) directly controls yaw.
// This is where control affects the target heading. It also (later) directly controls yaw.
headingError -= controls[CONTROL_YAW];
debugOut.analog[28] = headingError / 100;
if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT;
if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT;
if (headingError < -YAW_I_LIMIT)
headingError = -YAW_I_LIMIT;
if (headingError > YAW_I_LIMIT)
headingError = YAW_I_LIMIT;
 
PDPart = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
 
/*
* 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
}
// Lets not limit P and D.
// 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 yaw term.
* The yaw term is limited: Absolute value is max. = the throttle term / 2.
271,7 → 147,7
*/
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
// Limit yawTerm
// Limit yawTerm
debugOut.digital[0] &= ~DEBUG_CLIP;
if (throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm / 2) {
292,6 → 168,7
}
 
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
 
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
302,7 → 179,8
 
debugOut.digital[1] &= ~DEBUG_CLIP;
 
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6;
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
310,19 → 188,19
/************************************************************************/
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis = PITCH; axis <= ROLL; axis++) {
int16_t iDiff;
iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
// In HH mode, the I part is summed from P and D of gyros minus stick.
if (gyroIFactor) {
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
PDPart += iDiff;
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
} else {
IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
}
 
// With normal Ki, limit effect to +/- 205 (of 1024!!!)
// With normal Ki, limit I parts to +/- 205 (of about 1024)
if (IPart[axis] < -64000) {
IPart[axis] = -64000;
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
331,9 → 209,10
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
}
 
term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14;
term[axis] += (dynamicParams.levelCorrection[axis] - 128);
/*
term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
term[axis] += (dynamicParams.levelCorrection[axis] - 128);
 
/*
* 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).
341,8 → 220,10
*/
if (term[axis] < -tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = -tmp_int;
} else if (term[axis] > tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = tmp_int;
}
}
 
351,27 → 232,30
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
 
debugOut.analog[6] = filteredAcc[PITCH];
debugOut.analog[7] = filteredAcc[ROLL];
debugOut.analog[8] = filteredAcc[Z];
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
}
 
debugOut.analog[13] = term[PITCH];
debugOut.analog[14] = term[ROLL];
debugOut.analog[15] = yawTerm;
debugOut.analog[16] = throttleTerm;
debugOut.analog[8] = yawTerm;
debugOut.analog[9] = throttleTerm;
 
debugOut.analog[16] = gyroActivity;
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
uint8_t throttle;
 
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 = (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.
387,7 → 271,8
CHECK_MIN_MAX(tmp, 1, 255);
throttle = tmp;
 
// if (i < 4) debugOut.analog[22 + i] = throttle;
if (i < 4)
debugOut.analog[10 + i] = throttle;
 
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
motor[i].throttle = throttle;
399,14 → 284,4
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
}
}