0,0 → 1,607 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 excample: 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" |
#include "flight.h" |
|
// Only for debug. Remove. |
#include "analog.h" |
|
// Necessary for external control and motor test |
#include "uart0.h" |
#include "twimaster.h" |
#include "attitude.h" |
#include "controlMixer.h" |
#ifdef USE_MK3MAG |
#include "gps.h" |
#endif |
|
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
|
// TODO: These are no longer maintained, just left at 0. The original implementation just summed the acc. |
// 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; |
|
// MK flags |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 0; |
|
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
uint8_t yawRatePFactor, yawRateIFactor; // the PD factors for the yaw control |
|
// Some integral weight constant... |
uint16_t Ki = 10300 / 33; |
|
uint8_t RequiredMotors = 0; |
|
// No support for altitude control right |
// int16_t SetPointHeight = 0; |
|
/************************************************************************/ |
/* 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; |
} |
} |
|
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void flight_setNeutral() { |
// GPSStickPitch = 0; |
// GPSStickRoll = 0; |
|
MKFlags |= MKFLAG_CALIBRATE; |
|
// not really used here any more. |
dynamicParams.KalmanK = -1; |
dynamicParams.KalmanMaxDrift = 0; |
dynamicParams.KalmanMaxFusion = 32; |
|
controlMixer_initVariables(); |
|
// TODO: Move off. |
// RC_Quality = 100; |
} |
|
/************************************************************************/ |
/* Transmit Motor Data via I2C */ |
/************************************************************************/ |
void sendMotorData(void) { |
uint8_t i; |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
// If pilot has not started the engines.... |
MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off |
for(i = 0; i < MAX_MOTORS; i++) { |
// and if we are not in motor test mode, cut throttle on all motors. |
if(!motorTestActive) Motor[i].SetPoint = 0; |
else Motor[i].SetPoint = motorTest[i]; |
} |
if(motorTestActive) motorTestActive--; |
} |
|
DebugOut.Analog[12] = Motor[0].SetPoint; // Front |
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[14] = Motor[3].SetPoint; // Left |
DebugOut.Analog[15] = Motor[2].SetPoint; // Right |
// Start I2C Interrupt Mode |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
|
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Ki = 10300 / _Ki; |
gyroPFactor = _gyroPFactor; |
gyroIFactor = _gyroIFactor; |
yawRatePFactor = _yawPFactor; |
yawRateIFactor = _yawIFactor; |
} |
|
void setNormalFlightParameters(void) { |
if(staticParams.GlobalConfig & CFG_HEADING_HOLD) gyroIFactor = 0; |
else gyroIFactor = dynamicParams.GyroI; |
|
setFlightParameters(dynamicParams.IFactor + 1, |
dynamicParams.GyroP + 10, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? dynamicParams.GyroI : 0, |
dynamicParams.GyroP + 10, |
dynamicParams.UserParams[6] |
); |
} |
|
void setStableFlightParameters(void) { |
setFlightParameters(33, 90, 120, 90, 120); |
} |
|
void handleCommands(uint8_t command, uint8_t isCommandRepeated) { |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (command == COMMAND_GYROCAL && !isCommandRepeated) { |
// Run gyro calibration but do not repeat it. |
GRN_OFF; |
|
// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
// isFlying = 0; |
// check roll/pitch stick position |
// if pitch stick is top or roll stick is left or right --> change parameter setting |
// according to roll/pitch stick position |
|
uint8_t setting = controlMixer_getArgument(); |
|
if ((setting > 0 && setting < 6) || setting == 9) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
if(setting > 0 && setting < 6) { |
// A valid parameter-set (1..5) was chosen - use it. |
setActiveParamSet(setting); |
} |
ParamSet_ReadFromEEProm(getActiveParamSet()); |
attitude_setNeutral(); |
flight_setNeutral(); |
// Right stick is centered; calibrate it to zero (hmm strictly does not belong here). |
controlMixer_setNeutral(setting == 9); // Calibrate right stick neutral position. |
beepNumber(getActiveParamSet()); |
} else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) { |
// If right stick is centered and down |
compassCalState = 1; |
beep(1000); |
} |
} |
|
// save the ACC neutral setting to eeprom |
else { |
if(command == COMMAND_ACCCAL && !isCommandRepeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
GRN_OFF; |
analog_calibrateAcc(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(1); // Calibrate right stick neutral position. |
beepNumber(getActiveParamSet()); |
} |
} |
} // end !MOTOR_RUN condition. |
if (command == COMMAND_START) { |
isFlying = 1; // TODO: Really???? |
// if (!controlMixer_isCommandRepeated()) { |
// attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors. |
MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all??? |
// } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors. |
// attitude_continueDynamicCalibration(); |
// setPointYaw = 0; |
// IPartPitch = 0; |
// IPartRoll = 0; |
// } |
} else if (command == COMMAND_STOP) { |
isFlying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
|
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
int16_t tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, pitchTerm, rollTerm; |
|
// PID controller variables |
int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll; |
static int32_t IPartPitch = 0, IPartRoll = 0; |
|
static int32_t setPointYaw = 0; |
|
// 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; |
|
// No support for altitude control right now. |
// static uint8_t HeightControlActive = 0; |
// static int16_t HeightControlGas = 0; |
|
static int8_t debugDataTimer = 0; |
|
// High resolution motor values for smoothing of PID motor outputs |
static int16_t motorFilters[MAX_MOTORS]; |
|
uint8_t i; |
|
// Fire the main flight attitude calculation, including integration of angles. |
calculateFlightAttitude(); |
GRN_ON; |
|
/* |
* TODO: update should: Set the stick variables if good signal, set them to zero if bad. |
* Set variables also. |
*/ |
controlMixer_update(); |
|
throttleTerm = controlThrottle; |
if(throttleTerm < staticParams.GasMin + 10) throttleTerm = staticParams.GasMin + 10; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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 > 1000) { |
// 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 = 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. |
* TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already. |
*/ |
if(isFlying < 256) { |
IPartPitch = 0; |
IPartRoll = 0; |
// TODO: Don't stomp on other modules' variables!!! |
controlYaw = 0; |
if(isFlying == 250) { |
updateCompassCourse = 1; |
yawAngle = 0; |
setPointYaw = 0; |
} |
} else { |
// DebugOut.Digital[1] = 0; |
// 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); |
} |
|
/* |
* Get the current command (start/stop motors, calibrate), if any. |
*/ |
handleCommands(controlMixer_getCommand(), controlMixer_isCommandRepeated()); |
|
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
setNormalFlightParameters(); |
// } |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// set all inputs to save values |
/* |
* 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 latter. |
*/ |
if(looping) { |
if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
} |
|
/* |
* Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always |
* pushing of pulling on the pitch or roll stick. |
*/ |
/* |
if(ADCycleCount >= dynamicParams.UserParam2 * 10) { |
// This algo works OK on the desk but it is a little sluggish and it oscillates. |
// It does not very effectively cancel drift because of dynamics. |
|
minStickForAutoCal = dynamicParams.UserParam3 * 10; |
maxStickForAutoCal = dynamicParams.UserParam4 * 10; |
|
// If not already corrected to the limit, and dynamic calibration is enabled: |
if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
// The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset. |
if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) { |
dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...) |
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
} else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) { |
dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...) |
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
} |
} |
|
// If not already corrected to the limit, and dynamic calibration is enabled: |
if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) { |
dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128); |
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
} else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) { |
dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128); |
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
} |
} |
ADCycleCount = 0; |
} |
*/ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
badCompassHeading = 1000; |
if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
updateCompassCourse = 1; |
} |
} |
|
setPointYaw = controlYaw; |
|
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
// TODO: We want NO feedback of control related stuff to the attitude related stuff. |
yawAngle -= tmp_int; |
|
// limit the effect |
CHECK_MIN_MAX(yawAngle, -50000, 50000) |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// compass code is used if Compass option is selected |
|
/* |
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
updateCompass(); |
} |
*/ |
|
#if defined (USE_MK3MAG) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} |
else { |
// GPSStickPitch = 0; |
// GPSStickRoll = 0; |
} |
#endif |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// The P-part is actually the I-part... |
if(looping & LOOPING_PITCH_AXIS) { |
PPartPitch = 0; |
} else { |
// TODO: Where do the 44000 come from??? |
PPartPitch = (pitchAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral |
// PPartPitch = IntegralNickMalFaktor in H&I code. |
} |
|
// Now blend in the P-part - proportional to the Differential of the integral = the gyro value. |
PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor) / (256L / STICK_GAIN) |
+ (pitchDifferential * dynamicParams.GyroD) / 16; |
// = MesswertNick in H&I code |
|
// The P-part is actually the I-part... |
if(looping & LOOPING_ROLL_AXIS) { |
PPartRoll = 0; |
} else { |
PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral |
} |
|
// Now blend in the P-part - proportional to the Differential of the integral = the gyro value. |
PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor) / (256L / STICK_GAIN) |
+ (rollDifferential * dynamicParams.GyroD) / 16; |
|
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawRatePFactor) / (256L / STICK_GAIN) + (int32_t)(yawAngle * yawRateIFactor) / (2 * (44000 / STICK_GAIN)); |
|
// limit control feedback |
#define SENSOR_LIMIT (4096 * 4) |
CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT); |
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.GasMin; // reduce gas to min to avoid lift of |
} |
|
/* |
* Height control was here. |
*/ |
|
if(throttleTerm > staticParams.GasMax - 20) throttleTerm = (staticParams.GasMax - 20); |
throttleTerm *= STICK_GAIN; |
|
/* |
* Compose yaw term. |
*/ |
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value |
yawTerm = PDPartYaw - setPointYaw * STICK_GAIN; // yaw controller |
// 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)); |
} |
|
tmp_int = staticParams.GasMax * STICK_GAIN; |
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
|
/* |
* 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. |
IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos. |
IPartRoll += PPartRoll - controlRoll; // I-part for attitude control OK |
} else { |
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
IPartRoll += PDPartRoll - controlRoll; // 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(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
|
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki; // PID-controller for pitch |
|
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki; // PID-controller for roll |
|
/* |
* 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? |
*/ |
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int); |
CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) { |
int16_t tmp; |
if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered. |
tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
tmp += ((int32_t)pitchTerm * Mixer.Motor[i][MIX_PITCH]) / 64L; |
tmp += ((int32_t)rollTerm * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
tmp = motorFilters[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp, staticParams.GasMin, staticParams.GasMax); |
Motor[i].SetPoint = tmp; |
} |
else Motor[i].SetPoint = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!debugDataTimer--) { |
debugDataTimer = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
|
/* |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
*/ |
|
// 12..15 are the 4 first motors. |
|
DebugOut.Analog[16] = pitchAxisAcc; |
DebugOut.Analog[17] = rollAxisAcc; |
// DebugOut.Analog[18] = ZAxisAcc; |
DebugOut.Analog[19] = throttleTerm; |
DebugOut.Analog[20] = pitchTerm; |
DebugOut.Analog[21] = rollTerm; |
DebugOut.Analog[22] = yawTerm; |
DebugOut.Analog[23] = PPartPitch; // |
DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille. |
DebugOut.Analog[25] = PDPartPitch; // omtrent lig ppart. |
|
DebugOut.Analog[26] = pitchAccNoisePeak; |
DebugOut.Analog[27] = rollAccNoisePeak; |
|
DebugOut.Analog[30] = pitchGyroNoisePeak; |
DebugOut.Analog[31] = rollGyroNoisePeak; |
} |
} |