0,0 → 1,1903 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) Holger Buss, Ingo Busker |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "eeprom.h" |
#include "timer0.h" |
#include "analog.h" |
#include "printf_P.h" |
#include "fc.h" |
#include "uart0.h" |
#include "rc.h" |
#include "twimaster.h" |
#include "timer2.h" |
#include "mymath.h" |
#include "isqrt.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#include "gps.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#include "gps.h" |
#endif |
#include "led.h" |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
|
|
#define STICK_GAIN 4 |
#define LIMIT_MIN(value, min) {if(value < min) value = min;} |
#define LIMIT_MAX(value, max) {if(value > max) value = max;} |
#define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
|
// gyro readings |
int16_t GyroNick, GyroRoll, GyroYaw; |
|
// gyro bias |
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0; |
|
// accelerations |
int16_t AccNick, AccRoll, AccTop; |
|
// neutral acceleration readings |
int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0; |
volatile float AdBiasAccTop = 0; |
// the additive gyro rate corrections according to the axis coupling |
int16_t TrimNick, TrimRoll; |
|
|
// attitude gyro integrals |
int32_t IntegralGyroNick = 0,IntegralGyroNick2 = 0; |
int32_t IntegralGyroRoll = 0,IntegralGyroRoll2 = 0; |
int32_t IntegralGyroYaw = 0; |
int32_t ReadingIntegralGyroNick = 0, ReadingIntegralGyroNick2 = 0; |
int32_t ReadingIntegralGyroRoll = 0, ReadingIntegralGyroRoll2 = 0; |
int32_t ReadingIntegralGyroYaw = 0; |
int32_t MeanIntegralGyroNick; |
int32_t MeanIntegralGyroRoll; |
|
// attitude acceleration integrals |
int32_t MeanAccNick = 0, MeanAccRoll = 0; |
volatile int32_t ReadingIntegralTop = 0; |
|
// compass course |
int16_t CompassHeading = -1; // negative angle indicates invalid data. |
int16_t CompassCourse = -1; |
int16_t CompassOffCourse = 0; |
uint8_t CompassCalState = 0; |
uint8_t FunnelCourse = 0; |
uint16_t BadCompassHeading = 500; |
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t YawGyroDrift; |
|
|
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0; |
|
|
// MK flags |
uint16_t ModelIsFlying = 0; |
uint8_t volatile MKFlags = 0; |
|
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L; |
|
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control |
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
|
int16_t Ki = 10300 / 33; |
|
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
|
|
uint8_t RequiredMotors = 0; |
|
|
// stick values derived by rc channels readings |
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
int16_t GPSStickNick = 0, GPSStickRoll = 0; |
|
int16_t MaxStickNick = 0, MaxStickRoll = 0; |
|
// stick values derived by uart inputs |
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
|
int32_t SetPointHeight = 0; |
|
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0; |
|
uint8_t LoopingNick = 0, LoopingRoll = 0; |
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0; |
|
|
fc_param_t FCParam = {48,251,16,58,64,64,8,150,150,150,150,2,10,0,0,0,0,0,0,0,0,100,100,70,90,65,64,100,0,0,0}; |
|
|
|
/************************************************************************/ |
/* Filter for motor value smoothing */ |
/************************************************************************/ |
int16_t MotorSmoothing(int16_t newvalue, int16_t oldvalue) |
{ |
int16_t motor; |
if(newvalue > oldvalue) motor = (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
else motor = newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
return(motor); |
} |
|
/************************************************************************/ |
/* Creates numbeeps beeps at the speaker */ |
/************************************************************************/ |
void Beep(uint8_t numbeeps, uint16_t duration) |
{ |
if(MKFlags & MKFLAG_MOTOR_RUN) return; // never with running motors!!! |
while(numbeeps--) |
{ |
BeepTime = duration; // in ms second |
Delay_ms(duration * 2); // blocks 2 times beep duration, |
// this will block the flight control loop !!!!! |
// therefore do not use this function if motors are running |
} |
} |
|
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(uint8_t AccAdjustment) |
{ |
uint8_t i; |
int32_t Sum_1, Sum_2 = 0, Sum_3; |
|
//Servo_Off(); // disable servo output |
|
AdBiasAccNick = 0; |
AdBiasAccRoll = 0; |
AdBiasAccTop = 0; |
|
BiasHiResGyroNick = 0; |
BiasHiResGyroRoll = 0; |
AdBiasGyroYaw = 0; |
|
FCParam.AxisCoupling1 = 0; |
FCParam.AxisCoupling2 = 0; |
|
ExpandBaro = 0; |
|
// sample values with bias set to zero |
Delay_ms_Mess(100); |
|
if(BoardRelease == 13) SearchDacGyroOffset(); |
|
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR)) // air pressure sensor installed? |
{ |
if((AdAirPressure > AIR_PRESSURE_SEARCH_MAX) || (AdAirPressure < AIR_PRESSURE_SEARCH_MIN)) SearchAirPressureOffset(); |
} |
|
// determine gyro bias by averaging (require no rotation movement) |
#define GYRO_BIAS_AVERAGE 32 |
Sum_1 = 0; |
Sum_2 = 0; |
Sum_3 = 0; |
for(i=0; i < GYRO_BIAS_AVERAGE; i++) |
{ |
Delay_ms_Mess(10); |
Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY; |
Sum_2 += AdValueGyroRoll * HIRES_GYRO_AMPLIFY; |
Sum_3 += AdValueGyroYaw; |
} |
BiasHiResGyroNick = (int16_t)((Sum_1 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
|
if(AccAdjustment != NO_ACC_CALIB) |
{ |
// determine acc bias by averaging (require horizontal adjustment in nick and roll attitude) |
#define ACC_BIAS_AVERAGE 10 |
Sum_1 = 0; |
Sum_2 = 0; |
Sum_3 = 0; |
for(i=0; i < ACC_BIAS_AVERAGE; i++) |
{ |
Delay_ms_Mess(10); |
Sum_1 += AdValueAccNick; |
Sum_2 += AdValueAccRoll; |
Sum_3 += AdValueAccZ; |
} |
// use abs() to avoid negative bias settings because of adc sign flip in adc.c |
AdBiasAccNick = (int16_t)((abs(Sum_1) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
AdBiasAccRoll = (int16_t)((abs(Sum_2) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
AdBiasAccTop = (int16_t)((abs(Sum_3) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
|
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_NICK, (uint16_t)AdBiasAccNick); |
SetParamWord(PID_ACC_ROLL, (uint16_t)AdBiasAccRoll); |
SetParamWord(PID_ACC_TOP, (uint16_t)AdBiasAccTop); |
} |
else // restore from eeprom |
{ |
AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK); |
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL); |
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP); |
|
if((AdBiasAccNick > 2048) || (AdBiasAccRoll > 2048) || (AdBiasAccTop > 1024)) |
{ |
printf("\n\rACC not calibrated!\r\n"); |
AdBiasAccNick = 1024; |
AdBiasAccRoll = 1024; |
AdBiasAccTop = 725; |
} |
} |
// offset for height reading |
StartAirPressure = AirPressure; |
|
// setting acc bias values has an influence in the analog.c ISR |
// therefore run measurement for 100ms to achive stable readings |
Delay_ms_Mess(100); |
|
ReadingVario = 0; |
|
// reset acc averaging and integrals |
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick; |
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
AccTop = AdValueAccTop; |
ReadingIntegralTop = AdValueAccTop * 1024; |
|
// and gyro readings |
GyroNick = 0; |
GyroRoll = 0; |
GyroYaw = 0; |
|
// reset gyro integrals to acc guessing |
IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick; |
IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll; |
//ReadingIntegralGyroNick = IntegralGyroNick; |
//ReadingIntegralGyroRoll = IntegralGyroRoll; |
ReadingIntegralGyroNick2 = IntegralGyroNick; |
ReadingIntegralGyroRoll2 = IntegralGyroRoll; |
ReadingIntegralGyroYaw = 0; |
|
// update compass course to current heading |
CompassCourse = CompassHeading; |
// Inititialize YawGyroIntegral value with current compass heading |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
YawGyroDrift = 0; |
|
BeepTime = 50; |
|
TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
|
ExternHeightValue = 0; |
|
GPSStickNick = 0; |
GPSStickRoll = 0; |
|
MKFlags |= MKFLAG_CALIBRATE; |
|
FCParam.KalmanK = -1; |
FCParam.KalmanMaxDrift = 0; |
FCParam.KalmanMaxFusion = 32; |
|
Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET; |
Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET; |
Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET; |
Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET; |
|
//Servo_On(); //enable servo output |
RC_Quality = 100; |
} |
|
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
{ |
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0; |
int16_t FilterGyroNick, FilterGyroRoll; |
static int16_t Last_GyroRoll = 0, Last_GyroNick = 0; |
int16_t d2Nick, d2Roll; |
int32_t AngleNick, AngleRoll; |
int16_t CouplingNickRoll = 0, CouplingRollNick = 0; |
|
// Get bias free gyro readings |
GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate |
FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate |
|
// handle rotation rates that violate adc ranges |
if(AdValueGyroNick < 15) GyroNick = -1000; |
if(AdValueGyroNick < 7) GyroNick = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyroNick > 1010) GyroNick = +1000; |
if(AdValueGyroNick > 1017) GyroNick = +2000; |
} |
else |
{ |
if(AdValueGyroNick > 2000) GyroNick = +1000; |
if(AdValueGyroNick > 2015) GyroNick = +2000; |
} |
|
GyroRoll = HiResGyroRoll / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate |
FilterGyroRoll = FilterHiResGyroRoll / HIRES_GYRO_AMPLIFY; // use filtered gyro rate |
// handle rotation rates that violate adc ranges |
if(AdValueGyroRoll < 15) GyroRoll = -1000; |
if(AdValueGyroRoll < 7) GyroRoll = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyroRoll > 1010) GyroRoll = +1000; |
if(AdValueGyroRoll > 1017) GyroRoll = +2000; |
} |
else |
{ |
if(AdValueGyroRoll > 2000) GyroRoll = +1000; |
if(AdValueGyroRoll > 2015) GyroRoll = +2000; |
} |
|
GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
|
// Acceleration Sensor |
// lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution |
AccNick = ((int32_t)AccNick * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L; |
AccRoll = ((int32_t)AccRoll * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L; |
AccTop = ((int32_t)AccTop * 3L + ((int32_t)AdValueAccTop)) / 4L; |
|
// sum acc sensor readings for later averaging |
MeanAccNick += ACC_AMPLIFY * AdValueAccNick; |
MeanAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
|
NaviAccNick += AdValueAccNick; |
NaviAccRoll += AdValueAccRoll; |
NaviCntAcc++; |
|
|
// enable ADC to meassure next readings, before that point all variables should be read that are written by the ADC ISR |
ADC_Enable(); |
ADReady = 0; |
|
// limit angle readings for axis coupling calculations |
#define ANGLE_LIMIT 93000L // aprox. 93000/GYRO_DEG_FACTOR = 82 deg |
|
AngleNick = ReadingIntegralGyroNick; |
LIMIT_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT); |
|
AngleRoll = ReadingIntegralGyroRoll; |
LIMIT_MIN_MAX(AngleRoll, -ANGLE_LIMIT, ANGLE_LIMIT); |
|
|
// Yaw |
// calculate yaw gyro integral (~ to rotation angle) |
YawGyroHeading += GyroYaw; |
ReadingIntegralGyroYaw += GyroYaw; |
|
|
// Coupling fraction |
if(! LoopingNick && !LoopingRoll && (ParamSet.Config0 & CFG0_AXIS_COUPLING_ACTIVE)) |
{ |
tmp13 = (FilterGyroRoll * AngleNick) / 2048L; |
tmp13 *= FCParam.AxisCoupling2; |
tmp13 /= 4096L; |
CouplingNickRoll = tmp13; |
|
tmp14 = (FilterGyroNick * AngleRoll) / 2048L; |
tmp14 *= FCParam.AxisCoupling2; |
tmp14 /= 4096L; |
CouplingRollNick = tmp14; |
|
tmp14 -= tmp13; |
YawGyroHeading += tmp14; |
if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
|
tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L; |
tmpl *= FCParam.AxisCoupling1; |
tmpl /= 4096L; |
|
tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L; |
tmpl2 *= FCParam.AxisCoupling1; |
tmpl2 /= 4096L; |
if(abs(GyroYaw > 64)) |
{ |
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
} |
|
TrimNick = -tmpl2 + tmpl / 100L; |
TrimRoll = tmpl - tmpl2 / 100L; |
} |
else |
{ |
CouplingNickRoll = 0; |
CouplingRollNick = 0; |
TrimNick = 0; |
TrimRoll = 0; |
} |
|
|
// Yaw |
|
// limit YawGyroHeading proportional to 0° to 360° |
if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360° Wrap |
if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR; |
|
// Roll |
ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll; |
ReadingIntegralGyroRoll += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll; |
if(ReadingIntegralGyroRoll > TurnOver180Roll) |
{ |
ReadingIntegralGyroRoll = -(TurnOver180Roll - 10000L); |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
} |
if(ReadingIntegralGyroRoll < -TurnOver180Roll) |
{ |
ReadingIntegralGyroRoll = (TurnOver180Roll - 10000L); |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
} |
|
// Nick |
ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick; |
ReadingIntegralGyroNick += FilterGyroNick + TrimNick - AttitudeCorrectionNick; |
if(ReadingIntegralGyroNick > TurnOver180Nick) |
{ |
ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L); |
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
} |
if(ReadingIntegralGyroNick < -TurnOver180Nick) |
{ |
ReadingIntegralGyroNick = (TurnOver180Nick - 25000L); |
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
} |
|
IntegralGyroYaw = ReadingIntegralGyroYaw; |
IntegralGyroNick = ReadingIntegralGyroNick; |
IntegralGyroRoll = ReadingIntegralGyroRoll; |
IntegralGyroNick2 = ReadingIntegralGyroNick2; |
IntegralGyroRoll2 = ReadingIntegralGyroRoll2; |
|
|
#define D_LIMIT 128 |
|
if(FCParam.GyroD) |
{ |
d2Nick = (HiResGyroNick - Last_GyroNick); // change of gyro rate |
Last_GyroNick = (Last_GyroNick + HiResGyroNick) / 2; |
LIMIT_MIN_MAX(d2Nick, -D_LIMIT, D_LIMIT); |
GyroNick += (d2Nick * (int16_t)FCParam.GyroD) / 16; |
|
d2Roll = (HiResGyroRoll - Last_GyroRoll); // change of gyro rate |
Last_GyroRoll = (Last_GyroRoll + HiResGyroRoll) / 2; |
LIMIT_MIN_MAX(d2Roll, -D_LIMIT, D_LIMIT); |
GyroRoll += (d2Roll * (int16_t)FCParam.GyroD) / 16; |
|
HiResGyroNick += (d2Nick * (int16_t)FCParam.GyroD); |
HiResGyroRoll += (d2Roll * (int16_t)FCParam.GyroD); |
} |
|
// Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation |
if(FilterGyroNick > 0) TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L; |
else TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L; |
if(FilterGyroRoll > 0) TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L; |
else TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L; |
|
// increase the nick/roll rates virtually from the threshold of 245 to slow down higher rotation rates |
if((ParamSet.Config0 & CFG0_ROTARY_RATE_LIMITER) && ! LoopingNick && !LoopingRoll) |
{ |
if(FilterGyroNick > 256) GyroNick += 1 * (FilterGyroNick - 256); |
else if(FilterGyroNick < -256) GyroNick += 1 * (FilterGyroNick + 256); |
if(FilterGyroRoll > 256) GyroRoll += 1 * (FilterGyroRoll - 256); |
else if(FilterGyroRoll < -256) GyroRoll += 1 * (FilterGyroRoll + 256); |
} |
|
} |
|
|
/************************************************************************/ |
/* Transmit Motor Data via I2C */ |
/************************************************************************/ |
void SendMotorData(void) |
{ |
uint8_t i; |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) |
{ |
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if(!MotorTest_Active) Motor[i].SetPoint = 0; |
else Motor[i].SetPoint = MotorTest[i]; |
} |
if(MotorTest_Active) MotorTest_Active--; |
} |
|
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); |
} |
|
|
/************************************************************************/ |
/* Map the parameter to poti values */ |
/************************************************************************/ |
void ParameterMapping(void) |
{ |
if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok |
// else the last updated values are used |
{ |
//update poti values by rc-signals |
#define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
#define CHK_POTI(b,a) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a;} |
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight); |
CHK_POTI_MM(FCParam.HeightD,ParamSet.HeightD,0,100); |
CHK_POTI_MM(FCParam.HeightP,ParamSet.HeightP,0,100); |
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect); |
CHK_POTI(FCParam.Height_GPS_Z,ParamSet.Height_GPS_Z); |
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect); |
CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255); |
CHK_POTI(FCParam.GyroI,ParamSet.GyroI); |
CHK_POTI(FCParam.GyroD,ParamSet.GyroD); |
CHK_POTI_MM(FCParam.GyroYawP,ParamSet.GyroYawP,10,255); |
CHK_POTI(FCParam.GyroYawI,ParamSet.GyroYawI); |
CHK_POTI(FCParam.IFactor,ParamSet.IFactor); |
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1); |
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2); |
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3); |
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4); |
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5); |
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6); |
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7); |
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8); |
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl); |
CHK_POTI(FCParam.ServoRollControl,ParamSet.ServoRollControl); |
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit); |
CHK_POTI(FCParam.AxisCoupling1,ParamSet.AxisCoupling1); |
CHK_POTI(FCParam.AxisCoupling2,ParamSet.AxisCoupling2); |
CHK_POTI(FCParam.AxisCouplingYawCorrection,ParamSet.AxisCouplingYawCorrection); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability); |
CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255); |
CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255); |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl); |
CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain); |
CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP); |
CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI); |
CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD); |
CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC); |
CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255); |
CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection); |
CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation); |
#endif |
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl); |
Ki = 10300 / ( FCParam.IFactor + 1 ); |
} |
} |
|
|
void SetCompassCalState(void) |
{ |
static uint8_t stick = 1; |
|
// if nick is centered or top set stick to zero |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0; |
// if nick is down trigger to next cal state |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick) |
{ |
stick = 1; |
CompassCalState++; |
if(CompassCalState < 5) Beep(CompassCalState, 150); |
else BeepTime = 1000; |
} |
} |
|
|
|
/************************************************************************/ |
/* MotorControl */ |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t tmp_int1, tmp_int2; |
int32_t tmp_long, tmp_long2; |
|
// Mixer Fractions that are combined for Motor Control |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
|
// PID controller variables |
int16_t DiffNick, DiffRoll; |
int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll; |
static int32_t IPartNick = 0, IPartRoll = 0; |
|
static int32_t SetPointYaw = 0; |
static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0; |
static int32_t CorrectionNick, CorrectionRoll; |
static uint16_t RcLostTimer; |
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
static int8_t TimerDebugOut = 0; |
static uint16_t UpdateCompassCourse = 0; |
// high resolution motor values for smoothing of PID motor outputs |
static int16_t MotorValue[MAX_MOTORS]; |
uint8_t i; |
|
Mean(); |
GRN_ON; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality < 100) // the rc-frame signal is not reveived or noisy |
{ |
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
else // rc lost countdown finished |
{ |
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData() |
} |
RED_ON; // set red led |
if(ModelIsFlying > 1000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // set flag for emergency landing |
// set neutral rc inputs |
PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData() |
} // eof RC_Quality < 100 |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is good |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality > 140) |
{ |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
// reset emergency timer |
RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
#define GAS_FLIGHT_THRESHOLD 40 |
if(StickGas > GAS_FLIGHT_THRESHOLD && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
if(ModelIsFlying < 0xFFFF) ModelIsFlying++; |
} |
if(ModelIsFlying < 256) |
{ |
IPartNick = 0; |
IPartRoll = 0; |
StickYaw = 0; |
if(ModelIsFlying == 250) |
{ |
UpdateCompassCourse = 1; |
ReadingIntegralGyroYaw = 0; |
SetPointYaw = 0; |
} |
} |
else MKFlags |= MKFLAG_FLY; // set fly flag |
|
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET && Poti4) Poti4--; |
//PPM24-Extension |
if(Poti5 < PPM_in[9] + RC_POTI_OFFSET) Poti5++; else if(Poti5 > PPM_in[9] + RC_POTI_OFFSET && Poti5) Poti5--; |
if(Poti6 < PPM_in[10] + RC_POTI_OFFSET) Poti6++; else if(Poti6 > PPM_in[10] + RC_POTI_OFFSET && Poti6) Poti6--; |
if(Poti7 < PPM_in[11] + RC_POTI_OFFSET) Poti7++; else if(Poti7 > PPM_in[11] + RC_POTI_OFFSET && Poti7) Poti7--; |
if(Poti8 < PPM_in[12] + RC_POTI_OFFSET) Poti8++; else if(Poti8 > PPM_in[12] + RC_POTI_OFFSET && Poti8) Poti8--; |
//limit poti values |
#define POTI_MIN 0 |
#define POTI_MAX 255 |
LIMIT_MIN_MAX(Poti1, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti2, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti3, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti4, POTI_MIN, POTI_MAX); |
//PPM24-Extension |
LIMIT_MIN_MAX(Poti5, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti6, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti7, POTI_MIN, POTI_MAX); |
LIMIT_MIN_MAX(Poti8, POTI_MIN, POTI_MAX); |
|
// if motors are off and the gas stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
// and if the yaw stick is in the leftmost position |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calibrate the neutral readings of all attitude sensors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
{ |
// gas/yaw joystick is top left |
// _________ |
// |x | |
// | | |
// | | |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
ModelIsFlying = 0; |
// check roll/nick stick position |
// if nick stick is top or roll stick is left or right --> change parameter setting |
// according to roll/nick stick position |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
{ |
uint8_t setting = 1; // default |
// nick/roll joystick |
// _________ |
// |2 3 4| |
// | | |
// |1 5| |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
// roll stick leftmost and nick stick centered --> setting 1 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1; |
// roll stick leftmost and nick stick topmost --> setting 2 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2; |
// roll stick centered an nick stick topmost --> setting 3 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3; |
// roll stick rightmost and nick stick topmost --> setting 4 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4; |
// roll stick rightmost and nick stick centered --> setting 5 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5; |
// update active parameter set in eeprom |
SetActiveParamSet(setting); |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
Servo_Off(); // disable servo output |
SetNeutral(NO_ACC_CALIB); |
Servo_On(); // enable servo output |
Beep(GetActiveParamSet(), 120); |
} |
else |
{ |
if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE)) |
{ |
// if roll stick is centered and nick stick is down |
if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
{ |
// nick/roll joystick |
// _________ |
// | | |
// | | |
// | | |
// | | |
// | x | |
// ¯¯¯¯¯¯¯¯¯ |
// enable calibration state of compass |
CompassCalState = 1; |
BeepTime = 1000; |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
Servo_Off(); // disable servo output |
SetNeutral(NO_ACC_CALIB); |
Servo_On(); // enable servo output |
Beep(GetActiveParamSet(), 120); |
} |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
Servo_Off(); // disable servo output |
SetNeutral(NO_ACC_CALIB); |
Servo_On(); // enable servo output |
Beep(GetActiveParamSet(), 120); |
} |
} |
} |
} |
// and if the yaw stick is in the rightmost position |
// save the ACC neutral setting to eeprom |
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
// gas/yaw joystick is top right |
// _________ |
// | x| |
// | | |
// | | |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
ModelIsFlying = 0; |
Servo_Off(); // disable servo output |
SetNeutral(ACC_CALIB); |
Servo_On(); // enable servo output |
Beep(GetActiveParamSet(), 120); |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// gas stick is down |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
// gas/yaw joystick is bottom right |
// _________ |
// | | |
// | | |
// | | |
// | | |
// | x| |
// ¯¯¯¯¯¯¯¯¯ |
// Start Motors |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 200; // do not repeat if once executed |
ModelIsFlying = 1; |
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START |
SetPointYaw = 0; |
ReadingIntegralGyroYaw = 0; |
ReadingIntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick; |
ReadingIntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll; |
ReadingIntegralGyroNick2 = IntegralGyroNick; |
ReadingIntegralGyroRoll2 = IntegralGyroRoll; |
IPartNick = 0; |
IPartRoll = 0; |
} |
} |
else delay_startmotors = 0; // reset delay timer if sticks are not in this position |
|
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
{ |
// gas/yaw joystick is bottom left |
// _________ |
// | | |
// | | |
// | | |
// | | |
// |x | |
// ¯¯¯¯¯¯¯¯¯ |
// Stop Motors |
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_stopmotors = 200; // do not repeat if once executed |
ModelIsFlying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
} |
// remapping of paameters only if the signal rc-sigbnal conditions are good |
} // eof RC_Quality > 150 |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// new values from RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC |
{ |
static int16_t stick_nick = 0, stick_roll = 0; |
|
ParameterMapping(); // remapping params (online poti replacement) |
|
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
stick_nick = (stick_nick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4; |
stick_nick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD; |
StickNick = stick_nick - GPSStickNick; |
|
stick_roll = (stick_roll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4; |
stick_roll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD; |
StickRoll = stick_roll - GPSStickRoll; |
|
// mapping of yaw |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
#define YAW_DEAD_RANGE 2 |
// (range of -YAW_DEAD_RANGE .. YAW_DEAD_RANGE is set to zero, to avoid unwanted yaw trimming on compass correction) |
if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE)) |
{ |
if (StickYaw > YAW_DEAD_RANGE) StickYaw-= YAW_DEAD_RANGE; |
else if (StickYaw< -YAW_DEAD_RANGE) StickYaw += YAW_DEAD_RANGE; |
else StickYaw = 0; |
} |
|
// mapping of gas |
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_OFFSET;// shift to positive numbers |
|
// update gyro control loop factors |
GyroPFactor = FCParam.GyroP + 10; |
GyroIFactor = FCParam.GyroI; |
GyroYawPFactor = FCParam.GyroYawP + 10; |
GyroYawIFactor = FCParam.GyroYawI; |
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analog control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define EXTERNAL_CONTROL_THRESHOLD 128 |
#define EXTERNAL_CONTROL_MAXSTICK_LIMIT 100 |
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > EXTERNAL_CONTROL_THRESHOLD) |
{ |
StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.StickP; |
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.StickP; |
StickYaw += ExternControl.Yaw; |
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
// avoid negative gas value |
if(StickGas < 0) StickGas = 0; |
|
// disable I part of gyro control feedback |
if(ParamSet.Config0 & CFG0_HEADING_HOLD) GyroIFactor = 0; |
|
// update max stick positions for nick and roll |
if(abs(StickNick / STICK_GAIN) > MaxStickNick) |
{ |
MaxStickNick = abs(StickNick)/STICK_GAIN; |
LIMIT_MAX(MaxStickNick, EXTERNAL_CONTROL_MAXSTICK_LIMIT); |
} |
else MaxStickNick--; |
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll) |
{ |
MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
LIMIT_MAX(MaxStickRoll, EXTERNAL_CONTROL_MAXSTICK_LIMIT); |
} |
else MaxStickRoll--; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_LEFT) LoopingLeft = 1; |
else |
{ |
if(LoopingLeft) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingLeft = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_RIGHT) LoopingRight = 1; |
else |
{ |
if(LoopingRight) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingRight = 0; |
} |
} |
|
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_UP) LoopingTop = 1; |
else |
{ |
if(LoopingTop) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingTop = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.Config1 & CFG1_LOOP_DOWN) LoopingDown = 1; |
else |
{ |
if(LoopingDown) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0; |
} |
} |
|
if(LoopingLeft || LoopingRight) LoopingRoll = 1; else LoopingRoll = 0; |
if(LoopingTop || LoopingDown) { LoopingNick = 1; LoopingRoll = 0; LoopingLeft = 0; LoopingRight = 0;} else LoopingNick = 0; |
} // End of new RC-Values or Emergency Landing |
|
|
if(LoopingRoll || LoopingNick) |
{ |
LIMIT_MAX(StickGas, ParamSet.LoopGasLimit); |
FunnelCourse = 1; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// set all inputs to save values |
if(MKFlags & MKFLAG_EMERGENCY_LANDING) |
{ |
StickYaw = 0; |
StickNick = 0; |
StickRoll = 0; |
StickGas = ParamSet.EmergencyGas; |
GyroPFactor = 90; |
GyroIFactor = 120; |
GyroYawPFactor = 90; |
GyroYawIFactor = 120; |
LoopingRoll = 0; |
LoopingNick = 0; |
MaxStickNick = 0; |
MaxStickRoll = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Trim Gyro-Integrals to ACC-Signals |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
#define BALANCE_NUMBER 256L |
// sum for averaging |
MeanIntegralGyroNick += IntegralGyroNick; |
MeanIntegralGyroRoll += IntegralGyroRoll; |
|
if( LoopingNick || LoopingRoll) // if looping in any direction |
{ |
// reset averaging for acc and gyro integral as well as gyro integral acc correction |
MeasurementCounter = 0; |
|
MeanAccNick = 0; |
MeanAccRoll = 0; |
|
MeanIntegralGyroNick = 0; |
MeanIntegralGyroRoll = 0; |
|
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
|
AttitudeCorrectionNick = 0; |
AttitudeCorrectionRoll = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (MKFlags & MKFLAG_MOTOR_RUN) ) ) // if not lopping in any direction |
{ |
if( FCParam.KalmanK != -1) |
{ |
// determine the deviation of gyro integral from averaged acceleration sensor |
tmp_long = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick); |
tmp_long = (tmp_long * FCParam.KalmanK) / (32 * 16); |
tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll); |
tmp_long2 = (tmp_long2 * FCParam.KalmanK) / (32 * 16); |
|
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
{ |
tmp_long /= 2; |
tmp_long2 /= 2; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
// limit correction effect |
LIMIT_MIN_MAX(tmp_long, -(int32_t)FCParam.KalmanMaxFusion, (int32_t)FCParam.KalmanMaxFusion); |
LIMIT_MIN_MAX(tmp_long2, -(int32_t)FCParam.KalmanMaxFusion, (int32_t)FCParam.KalmanMaxFusion); |
} |
else |
{ |
// determine the deviation of gyro integral from acceleration sensor |
tmp_long = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick); |
tmp_long /= 16; |
tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll); |
tmp_long2 /= 16; |
|
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
|
#define BALANCE 32 |
// limit correction effect |
LIMIT_MIN_MAX(tmp_long, -BALANCE, BALANCE); |
LIMIT_MIN_MAX(tmp_long2, -BALANCE, BALANCE); |
} |
// correct current readings |
ReadingIntegralGyroNick -= tmp_long; |
ReadingIntegralGyroRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int16_t cnt = 0; |
static int8_t last_n_p, last_n_n, last_r_p, last_r_n; |
static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old; |
|
// if not lopping in any direction (this should be always the case, |
// because the Measurement counter is reset to 0 if looping in any direction is active.) |
if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp) |
{ |
// Calculate mean value of the gyro integrals |
MeanIntegralGyroNick /= BALANCE_NUMBER; |
MeanIntegralGyroRoll /= BALANCE_NUMBER; |
|
// Calculate mean of the acceleration values scaled to the gyro integrals |
MeanAccNick = (ParamSet.GyroAccFactor * MeanAccNick) / BALANCE_NUMBER; |
MeanAccRoll = (ParamSet.GyroAccFactor * MeanAccRoll) / BALANCE_NUMBER; |
|
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick); |
CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim; |
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll); |
CorrectionRoll = IntegralGyroRollError / ParamSet.GyroAccTrim; |
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER; |
|
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.KalmanK == -1) ) |
{ |
AttitudeCorrectionNick /= 2; |
AttitudeCorrectionRoll /= 2; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor) |
IntegralGyroNickError = IntegralGyroNick2 - IntegralGyroNick; |
ReadingIntegralGyroNick2 -= IntegralGyroNickError; |
// deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor) |
IntegralGyroRollError = IntegralGyroRoll2 - IntegralGyroRoll; |
ReadingIntegralGyroRoll2 -= IntegralGyroRollError; |
|
if(ParamSet.DriftComp) |
{ |
if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
} |
YawGyroDrift = 0; |
|
#define ERROR_LIMIT0 (BALANCE_NUMBER / 2) |
#define ERROR_LIMIT1 (BALANCE_NUMBER * 2) |
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16) |
#define MOVEMENT_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroNickError > ERROR_LIMIT1) cnt = 4; |
CorrectionNick = 0; |
if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroNickError > ERROR_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8); |
CorrectionNick = IntegralGyroNickError / 8; |
if(CorrectionNick > 5000) CorrectionNick = 5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_p = 1; |
} |
else last_n_p = 0; |
if(IntegralGyroNickError < -ERROR_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8); |
CorrectionNick = IntegralGyroNickError / 8; |
if(CorrectionNick < -5000) CorrectionNick = -5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_n = 1; |
} |
else last_n_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift; |
// correct Gyro Offsets |
if(IntegralGyroNickError > ERROR_LIMIT0) BiasHiResGyroNick += cnt; |
if(IntegralGyroNickError < -ERROR_LIMIT0) BiasHiResGyroNick -= cnt; |
|
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroRollError > ERROR_LIMIT1) cnt = 4; |
CorrectionRoll = 0; |
if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroRollError > ERROR_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8); |
CorrectionRoll = IntegralGyroRollError / 8; |
if(CorrectionRoll > 5000) CorrectionRoll = 5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_p = 1; |
} |
else last_r_p = 0; |
if(IntegralGyroRollError < -ERROR_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8); |
CorrectionRoll = IntegralGyroRollError / 8; |
if(CorrectionRoll < -5000) CorrectionRoll = -5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_n = 1; |
} |
else last_r_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
// correct Gyro Offsets |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift; |
if(IntegralGyroRollError > ERROR_LIMIT0) BiasHiResGyroRoll += cnt; |
if(IntegralGyroRollError < -ERROR_LIMIT0) BiasHiResGyroRoll -= cnt; |
|
} |
else // looping is active |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
FunnelCourse = 0; |
} |
|
// if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction |
if(!GyroIFactor) |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralGyroNick_old = MeanIntegralGyroNick; |
MeanIntegralGyroRoll_old = MeanIntegralGyroRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// reset variables used for next averaging |
MeanAccNick = 0; |
MeanAccRoll = 0; |
MeanIntegralGyroNick = 0; |
MeanIntegralGyroRoll = 0; |
MeasurementCounter = 0; |
} // end of averaging |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickYaw) > 15 ) // yaw stick is activated |
{ |
BadCompassHeading = 1000; |
if(!(ParamSet.Config0 & CFG0_COMPASS_FIX)) |
{ |
UpdateCompassCourse = 1; |
} |
} |
// exponential stick sensitivity in yawring rate |
tmp_int1 = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int1 += (ParamSet.StickYawP * StickYaw) / 4; |
SetPointYaw = tmp_int1; |
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
ReadingIntegralGyroYaw -= tmp_int1; |
// limit the effect |
LIMIT_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// compass code is used if Compass option is selected |
if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE)) |
{ |
int16_t w, v, r,correction, error; |
|
if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
SetCompassCalState(); |
#ifdef USE_KILLAGREG |
MM3_Calibrate(); |
#endif |
} |
else |
{ |
#ifdef USE_KILLAGREG |
static uint8_t updCompass = 0; |
if (!updCompass--) |
{ |
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
MM3_Heading(); |
} |
#endif |
|
// get maximum attitude angle |
w = abs(IntegralGyroNick / 512); |
v = abs(IntegralGyroRoll / 512); |
if(v > w) w = v; |
correction = w / 8 + 1; |
// calculate the deviation of the yaw gyro heading and the compass heading |
if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
if(abs(GyroYaw) > 128) // spinning fast |
{ |
error = 0; |
} |
if(!BadCompassHeading && w < 25) |
{ |
YawGyroDrift += error; |
if(UpdateCompassCourse) |
{ |
//BeepTime = 200; |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
UpdateCompassCourse = 0; |
} |
} |
YawGyroHeading += (error * 8) / correction; |
w = (w * FCParam.CompassYawEffect) / 32; |
w = FCParam.CompassYawEffect - w; |
if(w >= 0) |
{ |
if(!BadCompassHeading) |
{ |
v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
// calc course deviation |
r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * FCParam.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
ReadingIntegralGyroYaw += v; |
} |
else |
{ // wait a while |
BadCompassHeading--; |
} |
} |
else |
{ // ignore compass at extreme attitudes for a while |
BadCompassHeading = 500; |
} |
} |
} |
|
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ParamSet.Config0 & CFG0_GPS_ACTIVE) |
{ |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} |
else |
{ |
GPSStickNick = 0; |
GPSStickRoll = 0; |
} |
#endif |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// DebugOutputs |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[1] = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[2] = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[3] = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[4] = GyroYaw; |
DebugOut.Analog[5] = ReadingHeight/5; |
DebugOut.Analog[6] = (ReadingIntegralTop / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[19] = CompassCalState; |
DebugOut.Analog[20] = ServoNickValue; |
//DebugOut.Analog[29] = NCSerialDataOkay; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
#define TRIM_LIMIT 200 |
LIMIT_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT); |
LIMIT_MIN_MAX(TrimRoll, -TRIM_LIMIT, TRIM_LIMIT); |
|
if(FunnelCourse) |
{ |
IPartNick = 0; |
IPartRoll = 0; |
} |
|
if(! LoopingNick) |
{ |
PPartNick = (IntegralGyroNick * GyroIFactor) / (44000 / STICK_GAIN); // P-Part |
} |
else |
{ |
PPartNick = 0; |
} |
PDPartNick = PPartNick + (int32_t)((int32_t)GyroNick * GyroPFactor + (int32_t)TrimNick * 128L) / (256L / STICK_GAIN); // +D-Part |
|
if(!LoopingRoll) |
{ |
PPartRoll = (IntegralGyroRoll * GyroIFactor) / (44000 / STICK_GAIN); // P-Part |
} |
else |
{ |
PPartRoll = 0; |
} |
PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor + (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part |
|
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN)); |
|
// limit control feedback |
#define SENSOR_LIMIT (4096 * 4) |
LIMIT_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT); |
LIMIT_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT); |
LIMIT_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Height Control |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixFraction = StickGas; // take the direct stick command |
// at full LiPo the voltage is higher that gives more trust at the same BL-Control settpoint |
// therefore attenuate the gas proportional to the lipo voltage reserve over the low bat warning level |
// this yields to a nearly constant effective thrust over lipo discharging at the same stick position |
if(UBat > LowVoltageWarning) |
{ |
GasMixFraction = ((uint16_t)GasMixFraction * LowVoltageWarning) / UBat; |
} |
GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Airpressure sensor is enabled |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) ) |
{ |
#define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.1s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
|
int16_t CosAttitude; // for projection of hoover gas |
int16_t HCGas, HeightDeviation; |
static int16_t FilterHCGas = 0; |
static int16_t HeightTrimming = 0; // rate for change of height setpoint |
static uint8_t HCActive = 0; |
static int16_t StickGasHover = RC_GAS_OFFSET, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
static uint32_t HoverGasFilter = 0; |
static uint8_t delay = 100; |
|
#define BARO_LIMIT_MAX 0x01 |
#define BARO_LIMIT_MIN 0x02 |
#define BARO_EXPAND_TIME 350 // 350 * 2ms = 0.7s |
static uint8_t BaroFlags = 0; |
static uint16_t BaroExpandActive = 0; |
|
// get the current hoverpoint |
DebugOut.Analog[21] = HoverGas; |
DebugOut.Analog[18] = ReadingVario; |
|
// --------- barometer range expansion ------------------ |
if(BaroExpandActive) // delay, because of expanding the Baro-Range |
{ |
SumHeight = ReadingHeight * SM_FILTER; // reinit filter for vario |
ReadingVario = 0; |
// count down |
BaroExpandActive--; |
} |
else // expansion not active |
{ |
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
if(AdAirPressure > 923) |
{ // increase offset |
if(OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP)) |
{ |
ExpandBaro -= 1; |
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
BeepTime = 300; |
BaroExpandActive = BARO_EXPAND_TIME; |
} |
else |
{ |
BaroFlags |= BARO_LIMIT_MIN; |
} |
} |
// measurement of air pressure close to lower limit and |
else if(AdAirPressure < 100 ) |
{ // decrease offset |
if(OCR0A > EXPANDBARO_OPA_OFFSET_STEP) |
{ |
ExpandBaro += 1; |
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
BeepTime = 300; |
BaroExpandActive = BARO_EXPAND_TIME; |
} |
else |
{ |
BaroFlags |= BARO_LIMIT_MAX; |
} |
} |
else |
{ // still ok |
BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX); |
} |
}// EOF --------- barometer range expansion ------------------ |
|
|
// if height control is activated by an rc channel |
if(ParamSet.Config0 & CFG0_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
if( FCParam.MaxHeight < 50 ) // for 3 or 2-state switch height control is disabled in lowest position |
{ //height control not active |
if(!delay--) |
{ |
SetPointHeight = ReadingHeight; // update SetPoint with current reading |
HCActive = 0; // disable height control |
delay = 1; |
} |
} |
else |
{ //height control is activated |
HCActive = 1; // enable height control |
delay = 200; |
} |
} |
else // no switchable height control |
{ // the height control is always active and the set point is defined by the parameter |
if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) ) |
{ |
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain; |
} |
HCActive = 1; |
} |
|
|
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
tmp_int1 = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg |
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg |
CosAttitude = (int16_t)ihypot(tmp_int1, tmp_int2); |
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
|
if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
{ |
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
{ |
// Holgers original version |
// start of height control algorithm |
// the height control is only an attenuation of the actual gas stick. |
// I.e. it will work only if the gas stick is higher than the hover gas |
// and the hover height will be allways larger than height setpoint. |
|
HCGas = GasMixFraction; // take current stick gas as neutral point for the height control |
HeightTrimming = 0; |
} |
else // alternative height control |
{ |
// PD-Control with respect to hover point |
// the setpoint will be fine adjusted with the gas stick position |
#define HC_TRIM_UP 0x01 |
#define HC_TRIM_DOWN 0x02 |
static uint8_t HeightTrimmingFlag = 0x00; |
|
#define HC_STICKTHRESHOLD 15 |
|
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying |
{ // gas stick is above hover point |
if(StickGas > (StickGasHover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX)) |
{ |
if(HeightTrimmingFlag & HC_TRIM_DOWN) |
{ |
HeightTrimmingFlag &= ~HC_TRIM_DOWN; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
} |
HeightTrimmingFlag |= HC_TRIM_UP; |
HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD)); |
} // gas stick is below hover point |
else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
{ |
if(HeightTrimmingFlag & HC_TRIM_UP) |
{ |
HeightTrimmingFlag &= ~HC_TRIM_UP; |
SetPointHeight = ReadingHeight; // update setpoint to current heigth |
} |
HeightTrimmingFlag |= HC_TRIM_DOWN; |
HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD)); |
} |
else // gas stick in hover range |
{ |
if(HeightTrimmingFlag & (HC_TRIM_UP | HC_TRIM_DOWN)) |
{ |
HeightTrimmingFlag &= ~(HC_TRIM_UP | HC_TRIM_DOWN); |
HeightTrimming = 0; |
SetPointHeight = ReadingHeight; // update setpoint to current height |
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500; |
} |
} |
// trim height set point if needed |
if(abs(HeightTrimming) > 512) |
{ |
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/((5 * 512) / 2); // move setpoint |
HeightTrimming = 0; |
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75; |
//update hover gas stick value when setpoint is shifted |
if(!ParamSet.Height_StickNeutralPoint) |
{ |
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / LowVoltageWarning; |
LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
} |
} // EOF trimming height set point |
if(BaroExpandActive) SetPointHeight = ReadingHeight; // update setpoint to current altitude if expanding is active |
} //if MKFlags & MKFLAG_FLY |
else // not flying but height control is already active |
{ |
SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off |
if(ParamSet.Height_StickNeutralPoint) StickGasHover = ParamSet.Height_StickNeutralPoint; |
else StickGasHover = RC_GAS_OFFSET; |
} |
|
HCGas = HoverGas; // take hover gas (neutral point for PD controller) |
|
} //EOF alternative height control |
|
if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) ) |
{ |
// from this point the Heigth Control Algorithm is identical for both versions |
if(BaroExpandActive) // baro range expanding active |
{ |
HCGas = HoverGas; // hooer while expanding baro adc range |
} // EOF // baro range expanding active |
else // no baro range expanding |
{ |
// ------------------------- P-Part ---------------------------- |
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high |
tmp_int1 = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part |
HCGas -= tmp_int1; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int1 = ReadingVario / 8; |
if(tmp_int1 > 8) tmp_int1 = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
if(tmp_int1 > 0) tmp_int1 = ReadingVario + (tmp_int1 * tmp_int1) / 4; |
else tmp_int1 = ReadingVario - (tmp_int1 * tmp_int1) / 4; |
tmp_int1 = (FCParam.HeightD * (int32_t)(tmp_int1)) / 128L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
tmp_int1 = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
|
// limit deviation from hover point within the target region |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hover gas not zero |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hover point |
} |
} // EOF no baro range expanding |
|
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int1 = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int1, -127, 255); |
HCGas -= tmp_int1; |
|
// strech control output by inverse attitude projection 1/cos |
tmp_long2 = (int32_t)HCGas; |
tmp_long2 *= 8192L; |
tmp_long2 /= CosAttitude; |
HCGas = (int16_t)tmp_long2; |
|
// update height control gas averaging |
FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
// limit height control gas pd-control output |
LIMIT_MIN_MAX(FilterHCGas, ParamSet.HeightMinGas * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN); |
// limit gas to stick position for limiting height version |
if(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) |
{ |
LIMIT_MAX(FilterHCGas, GasMixFraction); |
} |
// set GasMixFraction to HeightControlGasFilter |
GasMixFraction = FilterHCGas; |
} // EOF if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT)) |
}// EOF height control active |
else // HC not active |
{ |
// update hover gas stick value when HC is not active |
if(ParamSet.Height_StickNeutralPoint) |
{ |
StickGasHover = ParamSet.Height_StickNeutralPoint; |
} |
else // take real hover stick position |
{ |
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value |
StickGasHover = (StickGasHover * UBat) / LowVoltageWarning; |
} |
LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down |
FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction |
} // EOF HC not active |
|
// ----------------- Hover Gas Estimation -------------------------------- |
// Hover gas estimation by averaging gas control output on small z-velocities |
// this is done only if height contol option is selected in global config and aircraft is flying |
if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING)) |
{ |
if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
if(abs(ReadingVario) < 100) // only on small vertical speed |
{ |
tmp_long2 = (int32_t)GasMixFraction; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
// average vertical projected thrust |
if(ModelIsFlying < 2000) // the first 4 seconds |
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L); |
HoverGasFilter += 8L * tmp_long2; |
} |
else if(ModelIsFlying < 4000) // the first 8 seconds |
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
HoverGasFilter += 4L * tmp_long2; |
} |
else if(ModelIsFlying < 8000) // the first 16 seconds |
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
HoverGasFilter += 2L * tmp_long2; |
} |
else //later |
{ |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |
} |
HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
if(ParamSet.Height_HoverBand) |
{ |
int16_t band; |
band = HoverGas / ParamSet.Height_HoverBand; // the higher the parameter the smaller the range |
HoverGasMin = HoverGas - band; |
HoverGasMax = HoverGas + band; |
} |
else |
{ // no limit |
HoverGasMin = 0; |
HoverGasMax = 1023; |
} |
} //EOF only on small vertical speed |
}// EOF ----------------- Hover Gas Estimation -------------------------------- |
|
}// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR |
|
// limit gas to parameter setting |
LIMIT_MIN_MAX(GasMixFraction, (ParamSet.GasMin + 10) * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// all BL-Ctrl connected? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(MissingMotor) |
{ |
// if we are in the lift off condition |
if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) ) |
ModelIsFlying = 1; // keep within lift off condition |
GasMixFraction = ParamSet.GasMin * STICK_GAIN; // reduce gas to min to avoid lift of |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mixer and PI-Controller |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMixFraction; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yaw-Fraction |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
YawMixFraction = PDPartYaw - SetPointYaw * STICK_GAIN; // yaw controller |
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value |
// limit YawMixFraction |
if(GasMixFraction > MIN_YAWGAS) |
{ |
LIMIT_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2)); |
} |
else |
{ |
LIMIT_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int1 = ParamSet.GasMax * STICK_GAIN; |
LIMIT_MIN_MAX(YawMixFraction, -(tmp_int1 - GasMixFraction), (tmp_int1 - GasMixFraction)); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = PDPartNick - StickNick; // get difference |
if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control |
else IPartNick += DiffNick; // I-part for head holding |
LIMIT_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = PDPartRoll - StickRoll; // get difference |
if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control |
else IPartRoll += DiffRoll; // I-part for head holding |
LIMIT_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
RollMixFraction = DiffRoll + (IPartRoll / Ki); // PID-controller for roll |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Limiter |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
tmp_int1 = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
LIMIT_MIN_MAX(NickMixFraction, -tmp_int1, tmp_int1); |
LIMIT_MIN_MAX(RollMixFraction, -tmp_int1, tmp_int1); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
int16_t tmp; |
if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer |
{ |
tmp = ((int32_t)GasMixFraction * Mixer.Motor[i][MIX_GAS] ) / 64L; |
tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L; |
tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)YawMixFraction * Mixer.Motor[i][MIX_YAW] ) / 64L; |
MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]); // Spike Filter |
tmp = MotorValue[i] / STICK_GAIN; |
LIMIT_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax); |
Motor[i].SetPoint = tmp; |
} |
else Motor[i].SetPoint = 0; |
} |
} |
|