WebSVN - FlightCtrl - Rev 1539 - /branches/V0.76g

/*#######################################################################################
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;
}
}

Subversion Repositories FlightCtrl

(root)/branches/V0.76g_Code Redesign killagreg/fc.c @ 1705 - Rev 1539