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; |
} |
} |