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