WebSVN - FlightCtrl - Rev 1240 - /branches/V0.74d_CRD_killagreg_Batteriekappazitaet

/*#######################################################################################
Flight Control
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "main.h"
#include "eeprom.h"
#include "timer0.h"
#include "analog.h"
#include "fc.h"
#include "uart0.h"
#include "rc.h"
#include "twimaster.h"
#include "timer2.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 CHECK_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;

int16_t ReadingHeight = 0;
int16_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,8,150,150,2,10,0,0,0,0,0,0,0,0,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)
{
while(numbeeps--)
{
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
BeepTime = 100; // 0.1 second
Delay_ms(250); // blocks 250 ms as pause to next beep,
// 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.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated?
{
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) 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)
{
// 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);
}
// 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);

// reset acc averaging and integrals
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
AccTop = AdValueAccTop;
ReadingIntegralTop = AdValueAccTop;

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

StartAirPressure = AirPressure;
HeightD = 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]] + 110;
Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110;
Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110;
Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110;

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 * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
AccRoll = ((int32_t)AccRoll * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
AccTop = ((int32_t)AccTop * 3 + ((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;
CHECK_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT);

AngleRoll = ReadingIntegralGyroRoll;
CHECK_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.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
{
tmp13 = (FilterGyroRoll * AngleNick) / 2048L;
tmp13 *= FCParam.AxisCoupling2; // 65
tmp13 /= 4096L;
CouplingNickRoll = tmp13;

tmp14 = (FilterGyroNick * AngleRoll) / 2048L;
tmp14 *= FCParam.AxisCoupling2; // 65
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;
CHECK_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;
CHECK_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.GlobalConfig & CFG_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.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(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.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);
else BeepTime = 1000;
}
}

/************************************************************************/
/* MotorControl */
/************************************************************************/
void MotorControl(void)
{
int16_t h, tmp_int;

// 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 uint8_t HeightControlActive = 0;
static int16_t HeightControlGas = 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;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// determine gas value
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMixFraction = StickGas;
if(GasMixFraction < ParamSet.GasMin + 10) GasMixFraction = ParamSet.GasMin + 10;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(RC_Quality < 120) // the rc-frame signal is not reveived or noisy
{
if(!PcAccess) // if also no PC-Access via UART
{
if(BeepModulation == 0xFFFF)
{
BeepTime = 15000; // 1.5 seconds
BeepModulation = 0x0C00;
}
}
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
{
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr 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 < 120
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;
if(GasMixFraction > 40 && (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]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
//PPM24-Extension
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
//limit poti values
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Extension
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;

// 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());
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
else
{
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_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());
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
}
else // nick and roll are centered
{
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
}
}
}
// 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;
SetNeutral(ACC_CALIB);
Beep(GetActiveParamSet());
}
}
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]];
// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
{
if (StickYaw > 2) StickYaw-= 2;
else if (StickYaw< -2) StickYaw += 2;
else StickYaw = 0;
}

// mapping of gas
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers

// update gyro control loop factors
GyroPFactor = FCParam.GyroP + 10;
GyroIFactor = FCParam.GyroI;
GyroYawPFactor = FCParam.GyroP + 10;
GyroYawIFactor = FCParam.GyroI;

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
{
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;
}
if(StickGas < 0) StickGas = 0;

// disable I part of gyro control feedback
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0;

// update max stick positions for nick and roll
if(abs(StickNick / STICK_GAIN) > MaxStickNick)
{
MaxStickNick = abs(StickNick)/STICK_GAIN;
if(MaxStickNick > 100) MaxStickNick = 100;
}
else MaxStickNick--;
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
{
MaxStickRoll = abs(StickRoll)/STICK_GAIN;
if(MaxStickRoll > 100) MaxStickRoll = 100;
}
else MaxStickRoll--;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_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.BitConfig & CFG_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.BitConfig & CFG_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.BitConfig & CFG_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)
{
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = 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;
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
{
int32_t tmp_long, tmp_long2;
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
if(tmp_long > (int32_t)FCParam.KalmanMaxFusion) tmp_long = (int32_t)FCParam.KalmanMaxFusion;
if(tmp_long < -(int32_t)FCParam.KalmanMaxFusion) tmp_long =-(int32_t)FCParam.KalmanMaxFusion;
if(tmp_long2 > (int32_t)FCParam.KalmanMaxFusion) tmp_long2 = (int32_t)FCParam.KalmanMaxFusion;
if(tmp_long2 <-(int32_t)FCParam.KalmanMaxFusion) tmp_long2 =-(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
CHECK_MIN_MAX(tmp_long, -BALANCE, BALANCE);
CHECK_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.GlobalConfig & CFG_COMPASS_FIX))
{
UpdateCompassCourse = 1;
}
}
// exponential stick sensitivity in yawring rate
tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx²
tmp_int += (ParamSet.StickYawP * StickYaw) / 4;
SetPointYaw = tmp_int;
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
ReadingIntegralGyroYaw -= tmp_int;
// limit the effect
CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// compass code is used if Compass option is selected
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_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.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else
{
GPSStickNick = 0;
GPSStickRoll = 0;
}
#endif

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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;
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[24] = GyroNick/2;
// DebugOut.Analog[25] = GyroRoll/2;
DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
// DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
DebugOut.Analog[30] = GPSStickNick;
DebugOut.Analog[31] = GPSStickRoll;
}

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#define TRIM_LIMIT 200
CHECK_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT);
CHECK_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));

//DebugOut.Analog[21] = PDPartNick;
//DebugOut.Analog[22] = PDPartRoll;

// limit control feedback
#define SENSOR_LIMIT (4096 * 4)
CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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; // reduce gas to min to avoid lift of
}

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The height control algorithm reduces the gas but does not increase the gas.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

GasMixFraction *= STICK_GAIN;

// if height control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
{
int tmp_int;
static uint8_t delay = 100;
// if height control is activated by an rc channel
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if(
( (ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && ( (FCParam.MaxHeight > 80) && (FCParam.MaxHeight < 140) ) )|| // for 3-state switch height control is only disabled in center position
(!(ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && (FCParam.MaxHeight < 50) ) // for 2-State switch height control is disabled in lower position
)
{ //hight control not active
if(!delay--)
{
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
if( (ReadingAirPressure > 1000) && (OCR0A < 255) )
{ // increase offset
if(OCR0A < 244)
{
ExpandBaro -= 10;
OCR0A = PressureSensorOffset - ExpandBaro;
}
else
{
OCR0A = 254;
}
BeepTime = 300;
delay = 250;
}
// measurement of air pressure close to lower limit and
else if( (ReadingAirPressure < 100) && (OCR0A > 1) )
{ // decrease offset
if(OCR0A > 10)
{
ExpandBaro += 10;
OCR0A = PressureSensorOffset - ExpandBaro;
}
else
{
OCR0A = 1;
}
BeepTime = 300;
delay = 250;
}
else
{
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading
HeightControlActive = 0; // disable height control
delay = 1;
}
}
}
else
{ //hight control not active
HeightControlActive = 1; // enable height control
delay = 200;
}
}
else // no switchable height control
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
HeightControlActive = 1;
}
// get current height
h = ReadingHeight;
// if current height is above the setpoint reduce gas
if((h > SetPointHeight) && HeightControlActive)
{
// height difference -> P control part
h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN);
h = GasMixFraction - h; // reduce gas
// height gradient --> D control part
//h -= (HeightD * FCParam.HeightD) / (8 / STICK_GAIN); // D control part
h -= (HeightD) / (8 / STICK_GAIN); // D control part
// acceleration sensor effect
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
if(tmp_int > 70 * STICK_GAIN) tmp_int = 70 * STICK_GAIN;
else if(tmp_int < -(70 * STICK_GAIN)) tmp_int = -(70 * STICK_GAIN);
h -= tmp_int;
// update height control gas
HeightControlGas = (HeightControlGas*15 + h) / 16;
// limit gas reduction
if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
{
if(GasMixFraction >= ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = ParamSet.HeightMinGas * STICK_GAIN;
// allows landing also if gas stick is reduced below min gas on height control
if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
}
// limit gas to stick setting
if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction;
GasMixFraction = HeightControlGas;
}
}
// limit gas to parameter setting
if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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)
{
CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
}
else
{
CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
tmp_int = ParamSet.GasMax * STICK_GAIN;
CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - 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
CHECK_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
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
RollMixFraction = DiffRoll + (IPartRoll / Ki); // PID-controller for roll

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Limiter
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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;
CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
Motor[i].SetPoint = tmp;
}
else Motor[i].SetPoint = 0;
}
}

Subversion Repositories FlightCtrl

(root)/branches/V0.74d_CRD_killagreg_Batteriekappazitaet_Lion/fc.c @ 2130 - Rev 1240