Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 935 → Rev 936

/branches/V0.70d Code Redesign killagreg/fc.c
74,14 → 74,13
#endif
#include "led.h"
 
volatile uint16_t I2CTimeout = 100;
// gyro readings
volatile int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
// gyro neutral readings
volatile int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
volatile int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
// mean accelerations
volatile int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
 
// neutral acceleration readings
volatile int16_t NeutralAccX=0, NeutralAccY=0;
88,17 → 87,17
volatile float NeutralAccZ = 0;
 
// attitude gyro integrals
volatile int32_t IntegralNick = 0,IntegralNick2 = 0;
volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
volatile int32_t IntegralYaw = 0;
volatile int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
volatile int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
volatile int32_t Reading_IntegralGyroYaw = 0;
volatile int32_t MeanIntegralNick;
volatile int32_t MeanIntegralRoll;
int32_t IntegralNick = 0,IntegralNick2 = 0;
int32_t IntegralRoll = 0,IntegralRoll2 = 0;
int32_t IntegralYaw = 0;
int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
int32_t Reading_IntegralGyroYaw = 0;
int32_t MeanIntegralNick;
int32_t MeanIntegralRoll;
 
// attitude acceleration integrals
volatile int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
volatile int32_t Reading_Integral_Top = 0;
 
// compass course
115,10 → 114,9
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 
 
// flags
uint8_t MotorsOn = 0;
uint8_t EmergencyLanding = 0;
// MK flags
uint16_t Model_Is_Flying = 0;
volatile uint8_t MKFlags = 0;
 
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 
125,7 → 123,7
float Gyro_P_Factor;
float Gyro_I_Factor;
 
volatile int16_t DiffNick, DiffRoll;
int16_t DiffNick, DiffRoll;
 
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 
164,7 → 162,7
{
while(numbeeps--)
{
if(MotorsOn) return; //auf keinen Fall im Flug!
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,
217,6 → 215,7
Reading_GyroNick = 0;
Reading_GyroRoll = 0;
Reading_GyroYaw = 0;
Delay_ms_Mess(100);
StartAirPressure = AirPressure;
HeightD = 0;
Reading_Integral_Top = 0;
229,6 → 228,7
GPS_Roll = 0;
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
YawGyroDrift = 0;
MKFlags |= MKFLAG_CALIBRATE;
}
 
/************************************************************************/
356,6 → 356,7
/************************************************************************/
void CalibMean(void)
{
if(BoardRelease >= 13) SearchGyroOffset();
// stop ADC to avoid changing values during calculation
ADC_Disable();
 
379,7 → 380,7
/************************************************************************/
void SendMotorData(void)
{
if(MOTOR_OFF || !MotorsOn)
if(!(MKFlags & MKFLAG_MOTOR_RUN))
{
Motor_Rear = 0;
Motor_Front = 0;
389,8 → 390,8
if(MotorTest[1]) Motor_Rear = MotorTest[1];
if(MotorTest[2]) Motor_Left = MotorTest[2];
if(MotorTest[3]) Motor_Right = MotorTest[3];
}
 
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
}
DebugOut.Analog[12] = Motor_Front;
DebugOut.Analog[13] = Motor_Rear;
DebugOut.Analog[14] = Motor_Left;
397,8 → 398,7
DebugOut.Analog[15] = Motor_Right;
 
//Start I2C Interrupt Mode
twi_state = 0;
motor = 0;
twi_state = TWI_STATE_MOTOR_TX;
I2C_Start();
}
 
413,28 → 413,38
// else the last updated values are used
{
//update poti values by rc-signals
#define CHK_POTI(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;}
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl,0,255);
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
#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.Height_D,ParamSet.Height_D,0,100);
CHK_POTI_MM(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
CHK_POTI_MM(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor);
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.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
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(FCParam.ExternalControl,ParamSet.ExternalControl);
Ki = (float) FCParam.I_Factor * FACTOR_I;
}
}
501,14 → 511,13
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost
else // rc lost countdown finished
{
MotorsOn = 0; // stop all motors
EmergencyLanding = 0; // emergency landing is over
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
}
ROT_ON; // set red led
RED_ON; // set red led
if(Model_Is_Flying > 1000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
EmergencyLanding = 1; // enable emergency landing
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;
517,7 → 526,7
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
}
else MotorsOn = 0; // switch of all motors
else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData()
} // eof RC_Quality < 120
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
525,10 → 534,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(RC_Quality > 140)
{
EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
// reset emergency timer
RcLostTimer = ParamSet.EmergencyGasDuration * 50;
if(GasMixFraction > 40)
if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
{
if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
}
537,8 → 546,14
SumNick = 0;
SumRoll = 0;
StickYaw = 0;
if(Model_Is_Flying == 250) UpdateCompassCourse = 1;
if(Model_Is_Flying == 250)
{
UpdateCompassCourse = 1;
Reading_IntegralGyroYaw = 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--;
561,7 → 576,7
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) && MotorsOn == 0)
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)
681,7 → 696,7
{
delay_startmotors = 200; // do not repeat if once executed
Model_Is_Flying = 1;
MotorsOn = 1;
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
SetPointYaw = 0;
Reading_IntegralGyroYaw = 0;
Reading_IntegralGyroNick = 0;
690,12 → 705,6
Reading_IntegralGyroRoll2 = IntegralRoll;
SumNick = 0;
SumRoll = 0;
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_SetHomePosition();
}
#endif
}
}
else delay_startmotors = 0; // reset delay timer if sticks are not in this position
708,13 → 717,7
{
delay_stopmotors = 200; // do not repeat if once executed
Model_Is_Flying = 0;
MotorsOn = 0;
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_ClearHomePosition();
}
#endif
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
}
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
724,7 → 727,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC
{
int tmp_int;
ParameterMapping(); // remapping params (online poti replacement)
749,7 → 752,7
// Digital Control via DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#define KEY_VALUE (FCParam.UserParam8 * 4) // step width
#define KEY_VALUE (FCParam.ExternalControl * 4) // step width
if(DubWiseKeys[1]) BeepTime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE;
765,7 → 768,7
if(DubWiseKeys[0] & 2) ExternHeightValue++;
if(DubWiseKeys[0] & 16) ExternHeightValue--;
 
StickNick += (STICK_GAIN * ExternStickNick) / 8;
StickNick += (STICK_GAIN * ExternStickNick) / 8;
StickRoll += (STICK_GAIN * ExternStickRoll) / 8;
StickYaw += (STICK_GAIN * ExternStickYaw);
 
773,7 → 776,7
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(ExternControl.Config & 0x01 && FCParam.UserParam8 > 128)
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
{
StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P;
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
792,9 → 795,17
 
// update max stick positions for nick and roll
 
if(abs(StickNick / STICK_GAIN) > MaxStickNick) MaxStickNick = abs(StickNick)/STICK_GAIN;
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(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
{
MaxStickRoll = abs(StickRoll)/STICK_GAIN;
if(MaxStickRoll > 100) MaxStickRoll = 100;
}
else MaxStickRoll--;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
845,19 → 856,11
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit;
}
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ LED Control on J16/J17
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
LED1_Time = FCParam.UserParam7;
LED2_Time = FCParam.UserParam8;
LED_Update();
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// set all inputs to save values
if(EmergencyLanding)
if(MKFlags & MKFLAG_EMERGENCY_LANDING)
{
StickYaw = 0;
StickNick = 0;
907,7 → 910,7
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
 
if((MaxStickNick > 32) || (MaxStickRoll > 32)) // reduce effect during stick commands
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long /= 3;
tmp_long2 /= 3;
959,7 → 962,7
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
 
if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
1026,7 → 1029,7
else
{
cnt = 0;
BadCompassHeading = 500;
BadCompassHeading = 1000;
}
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
// correct Gyro Offsets
1066,7 → 1069,7
else
{
cnt = 0;
BadCompassHeading = 500;
BadCompassHeading = 1000;
}
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1109,11 → 1112,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickYaw) > 15 ) // yaw stick is activated
{
BadCompassHeading = 1000;
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
{
UpdateCompassCourse = 1;
CompassCourse = YawGyroHeading;
BadCompassHeading = 250;
}
}
// exponential stick sensitivity in yawring rate
1134,7 → 1136,7
{
int16_t w, v, r,correction, error;
 
if(CompassCalState && MotorsOn == 0 )
if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
{
SetCompassCalState();
#ifdef USE_KILLAGREG
1153,32 → 1155,31
#endif
 
// get maximum attitude angle
w = abs(IntegralNick/512);
v = abs(IntegralRoll /512);
w = abs(IntegralNick / 512);
v = abs(IntegralRoll / 512);
if(v > w) w = v;
// update compass course
if (w < 25 && UpdateCompassCourse && !BadCompassHeading)
{
BeepTime = 200;
CompassCourse = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
UpdateCompassCourse = 0;
}
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 / YAW_GYRO_DEG_FACTOR)) % 360) - 180;
correction = w / 8 + 1;
 
if(!BadCompassHeading && w < 25)
{
YawGyroDrift += error;
if(UpdateCompassCourse)
{
BeepTime = 200;
CompassCourse = (YawGyroHeading / YAW_GYRO_DEG_FACTOR);
UpdateCompassCourse = 0;
}
}
YawGyroHeading += (error * 8) / correction;
w = (w * FCParam.CompassYawEffect) / 64;
w = (w * FCParam.CompassYawEffect) / 32;
w = FCParam.CompassYawEffect - w;
if(w > 0)
if(w >= 0)
{
if(BadCompassHeading)
{ // wait a while
BadCompassHeading--;
}
else
{ //
YawGyroDrift += error;
if(!BadCompassHeading)
{
v = 64 + (MaxStickNick + MaxStickRoll) / 8;
// calc course deviation
r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
1189,10 → 1190,14
else if (v < -w) v = -w;
Reading_IntegralGyroYaw += v;
}
else
{ // wait a while
BadCompassHeading--;
}
}
else
{ // ignore compass at extreme attitudes for a while
BadCompassHeading = 250;
BadCompassHeading = 500;
}
}
}
1203,11 → 1208,8
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_I_Factor = FCParam.UserParam2;
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
if(EmergencyLanding) GPS_Main(230); // enables Comming Home
else GPS_Main(Poti3); // behavior controlled by Poti3
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else
{
1233,7 → 1235,7
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
DebugOut.Analog[16] = Mean_AccTop;
//DebugOut.Analog[16] = Mean_AccTop;
 
DebugOut.Analog[20] = ServoValue;
 
1297,7 → 1299,7
GasMixFraction *= STICK_GAIN;
 
// If height control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
{
int tmp_int;
// if height control is activated by an rc channel
1390,7 → 1392,7
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
 
1399,7 → 1401,7
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
1419,7 → 1421,7
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Left = MotorValue;
 
// Motor Right
1427,7 → 1429,7
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Right = MotorValue;
}