69,7 → 69,7 |
|
volatile unsigned int I2CTimeout = 100; |
// gyro readings |
volatile int16_t ReadingPitch, ReadingRoll, ReadingYaw; |
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
// gyro neutral readings |
volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0; |
volatile int16_t StartNeutralRoll = 0, StartNeutralPitch = 0; |
84,14 → 84,14 |
volatile int32_t IntegralPitch = 0,IntegralPitch2 = 0; |
volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0; |
volatile int32_t IntegralYaw = 0; |
volatile int32_t Reading_IntegralPitch = 0, Reading_IntegralPitch2 = 0; |
volatile int32_t Reading_IntegralRoll = 0, Reading_IntegralRoll2 = 0; |
volatile int32_t Reading_IntegralYaw = 0, Reading_IntegralYaw2 = 0; |
volatile int32_t MeanIntegralPitch, MeanIntegralPitch2; |
volatile int32_t MeanIntegralRoll, MeanIntegralRoll2; |
volatile int32_t Reading_IntegralGyroPitch = 0, Reading_IntegralGyroPitch2 = 0; |
volatile int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0; |
volatile int32_t Reading_IntegralGyroYaw = 0, Reading_IntegralGyroYaw2 = 0; |
volatile int32_t MeanIntegralPitch; |
volatile int32_t MeanIntegralRoll; |
|
// attitude acceleration integrals |
volatile int32_t IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0, IntegralAccZ = 0; |
volatile int32_t Reading_Integral_Top = 0; |
|
// compass course |
116,12 → 116,13 |
|
// stick values derived by rc channels readings |
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0; |
// stick values derived by uart inputs |
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
|
int MaxStickPitch = 0, MaxStickRoll = 0; |
|
|
|
int16_t ReadingHight = 0; |
int16_t SetPointHight = 0; |
|
188,14 → 189,14 |
NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z); |
} |
Reading_IntegralPitch = 0; |
Reading_IntegralPitch2 = 0; |
Reading_IntegralRoll = 0; |
Reading_IntegralRoll2 = 0; |
Reading_IntegralYaw = 0; |
ReadingPitch = 0; |
ReadingRoll = 0; |
ReadingYaw = 0; |
Reading_IntegralGyroPitch = 0; |
Reading_IntegralGyroPitch2 = 0; |
Reading_IntegralGyroRoll = 0; |
Reading_IntegralGyroRoll2 = 0; |
Reading_IntegralGyroYaw = 0; |
Reading_GyroPitch = 0; |
Reading_GyroRoll = 0; |
Reading_GyroYaw = 0; |
StartAirPressure = AirPressure; |
HightD = 0; |
Reading_Integral_Top = 0; |
214,112 → 215,115 |
{ |
static int32_t tmpl,tmpl2; |
|
// Get offset corrected gyro readings |
ReadingYaw = AdNeutralYaw - AdValueGyrYaw; |
ReadingRoll = AdValueGyrRoll - AdNeutralRoll; |
ReadingPitch = AdValueGyrPitch - AdNeutralPitch; |
// Get offset corrected gyro readings (~ to angular velocity) |
Reading_GyroYaw = AdNeutralYaw - AdValueGyrYaw; |
Reading_GyroRoll = AdValueGyrRoll - AdNeutralRoll; |
Reading_GyroPitch = AdValueGyrPitch - AdNeutralPitch; |
|
DebugOut.Analog[26] = ReadingPitch; |
DebugOut.Analog[28] = ReadingRoll; |
DebugOut.Analog[26] = Reading_GyroPitch; |
DebugOut.Analog[28] = Reading_GyroRoll; |
|
// Acceleration Sensor |
// sliding average sensor readings |
Mean_AccPitch = ((int32_t)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccPitch))) / 2L; |
Mean_AccRoll = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L; |
Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L; |
|
// sum sensor readings for later averaging |
IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch; |
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
IntegralAccZ += Current_AccZ - NeutralAccZ; |
|
// Yaw |
Reading_IntegralYaw += ReadingYaw; |
Reading_IntegralYaw2 += ReadingYaw; |
|
// Coupling fraction |
// calculate yaw gyro intergral (~ to rotation angle) |
Reading_IntegralGyroYaw += Reading_GyroYaw; |
Reading_IntegralGyroYaw2 += Reading_GyroYaw; |
// Coupling fraction |
if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
tmpl = Reading_IntegralPitch / 4096L; |
tmpl *= ReadingYaw; |
tmpl = Reading_IntegralGyroPitch / 4096L; |
tmpl *= Reading_GyroYaw; |
tmpl *= FCParam.Yaw_PosFeedback; //125 |
tmpl /= 2048L; |
tmpl2 = Reading_IntegralRoll / 4096L; |
tmpl2 *= ReadingYaw; |
tmpl2 = Reading_IntegralGyroRoll / 4096L; |
tmpl2 *= Reading_GyroYaw; |
tmpl2 *= FCParam.Yaw_PosFeedback; |
tmpl2 /= 2048L; |
} |
else tmpl = tmpl2 = 0; |
|
// Roll |
ReadingRoll += tmpl; |
ReadingRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; //109 |
Reading_IntegralRoll2 += ReadingRoll; |
Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
if(Reading_IntegralRoll > TurnOver180Roll) |
Reading_GyroRoll += tmpl; |
Reading_GyroRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; //109 |
Reading_IntegralGyroRoll2 += Reading_GyroRoll; |
Reading_IntegralGyroRoll += Reading_GyroRoll - AttitudeCorrectionRoll; |
if(Reading_IntegralGyroRoll > TurnOver180Roll) |
{ |
Reading_IntegralRoll = -(TurnOver180Roll - 10000L); |
Reading_IntegralRoll2 = Reading_IntegralRoll; |
Reading_IntegralGyroRoll = -(TurnOver180Roll - 10000L); |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
} |
if(Reading_IntegralRoll < -TurnOver180Roll) |
if(Reading_IntegralGyroRoll < -TurnOver180Roll) |
{ |
Reading_IntegralRoll = (TurnOver180Roll - 10000L); |
Reading_IntegralRoll2 = Reading_IntegralRoll; |
Reading_IntegralGyroRoll = (TurnOver180Roll - 10000L); |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
} |
if(AdValueGyrRoll < 15) ReadingRoll = -1000; |
if(AdValueGyrRoll < 7) ReadingRoll = -2000; |
if(AdValueGyrRoll < 15) Reading_GyroRoll = -1000; |
if(AdValueGyrRoll < 7) Reading_GyroRoll = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyrRoll > 1010) ReadingRoll = +1000; |
if(AdValueGyrRoll > 1017) ReadingRoll = +2000; |
} |
else |
{ |
if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
} |
{ |
if(AdValueGyrRoll > 1010) Reading_GyroRoll = +1000; |
if(AdValueGyrRoll > 1017) Reading_GyroRoll = +2000; |
} |
else |
{ |
if(AdValueGyrRoll > 2020) Reading_GyroRoll = +1000; |
if(AdValueGyrRoll > 2034) Reading_GyroRoll = +2000; |
} |
// Pitch |
ReadingPitch -= tmpl2; |
ReadingPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
Reading_IntegralPitch2 += ReadingPitch; |
Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
if(Reading_IntegralPitch > TurnOver180Pitch) |
Reading_GyroPitch -= tmpl2; |
Reading_GyroPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
Reading_IntegralGyroPitch2 += Reading_GyroPitch; |
Reading_IntegralGyroPitch += Reading_GyroPitch - AttitudeCorrectionPitch; |
if(Reading_IntegralGyroPitch > TurnOver180Pitch) |
{ |
Reading_IntegralPitch = -(TurnOver180Pitch - 10000L); |
Reading_IntegralPitch2 = Reading_IntegralPitch; |
Reading_IntegralGyroPitch = -(TurnOver180Pitch - 10000L); |
Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch; |
} |
if(Reading_IntegralPitch < -TurnOver180Pitch) |
if(Reading_IntegralGyroPitch < -TurnOver180Pitch) |
{ |
Reading_IntegralPitch = (TurnOver180Pitch - 10000L); |
Reading_IntegralPitch2 = Reading_IntegralPitch; |
Reading_IntegralGyroPitch = (TurnOver180Pitch - 10000L); |
Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch; |
} |
if(AdValueGyrPitch < 15) ReadingPitch = -1000; |
if(AdValueGyrPitch < 7) ReadingPitch = -2000; |
if(AdValueGyrPitch < 15) Reading_GyroPitch = -1000; |
if(AdValueGyrPitch < 7) Reading_GyroPitch = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyrPitch > 1010) ReadingPitch = +1000; |
if(AdValueGyrPitch > 1017) ReadingPitch = +2000; |
} |
else |
{ |
if(AdValueGyrPitch > 2020) ReadingPitch = +1000; |
if(AdValueGyrPitch > 2034) ReadingPitch = +2000; |
} |
{ |
if(AdValueGyrPitch > 1010) Reading_GyroPitch = +1000; |
if(AdValueGyrPitch > 1017) Reading_GyroPitch = +2000; |
} |
else |
{ |
if(AdValueGyrPitch > 2020) Reading_GyroPitch = +1000; |
if(AdValueGyrPitch > 2034) Reading_GyroPitch = +2000; |
} |
|
// start ADC |
ADC_Enable(); |
|
IntegralYaw = Reading_IntegralYaw; |
IntegralPitch = Reading_IntegralPitch; |
IntegralRoll = Reading_IntegralRoll; |
IntegralPitch2 = Reading_IntegralPitch2; |
IntegralRoll2 = Reading_IntegralRoll2; |
IntegralYaw = Reading_IntegralGyroYaw; |
IntegralPitch = Reading_IntegralGyroPitch; |
IntegralRoll = Reading_IntegralGyroRoll; |
IntegralPitch2 = Reading_IntegralGyroPitch2; |
IntegralRoll2 = Reading_IntegralGyroRoll2; |
|
if(ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER && !Looping_Pitch && !Looping_Roll) |
if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Pitch && !Looping_Roll) |
{ |
if(ReadingPitch > 200) ReadingPitch += 4 * (ReadingPitch - 200); |
else if(ReadingPitch < -200) ReadingPitch += 4 * (ReadingPitch + 200); |
if(ReadingRoll > 200) ReadingRoll += 4 * (ReadingRoll - 200); |
else if(ReadingRoll < -200) ReadingRoll += 4 * (ReadingRoll + 200); |
if(Reading_GyroPitch > 200) Reading_GyroPitch += 4 * (Reading_GyroPitch - 200); |
else if(Reading_GyroPitch < -200) Reading_GyroPitch += 4 * (Reading_GyroPitch + 200); |
if(Reading_GyroRoll > 200) Reading_GyroRoll += 4 * (Reading_GyroRoll - 200); |
else if(Reading_GyroRoll < -200) Reading_GyroRoll += 4 * (Reading_GyroRoll + 200); |
} |
//update poti values by rc-signals (why not +127?) |
//update poti values by rc-signals |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
339,14 → 343,14 |
// stop ADC to avoid changing values during calculation |
ADC_Disable(); |
|
ReadingPitch = AdValueGyrPitch; |
ReadingRoll = AdValueGyrRoll; |
ReadingYaw = AdValueGyrYaw; |
Reading_GyroPitch = AdValueGyrPitch; |
Reading_GyroRoll = AdValueGyrRoll; |
Reading_GyroYaw = AdValueGyrYaw; |
|
Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch; |
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
Mean_AccTop = (int32_t)AdValueAccTop; |
// start ADC |
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
Mean_AccTop = (int32_t)AdValueAccTop; |
// start ADC |
ADC_Enable(); |
//update poti values by rc-signals (why not +127?) |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
399,30 → 403,30 |
void ParameterMapping(void) |
{ |
|
#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.MaxHight,ParamSet.MaxHight,0,255); |
CHK_POTI(FCParam.AirPressure_D,ParamSet.AirPressure_D,0,100); |
CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100); |
CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_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.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,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(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.MaxHight,ParamSet.MaxHight,0,255); |
CHK_POTI(FCParam.Hight_D,ParamSet.Hight_D,0,100); |
CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100); |
CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_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.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,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); |
|
Ki = (float) FCParam.I_Factor * FACTOR_I; |
Ki = (float) FCParam.I_Factor * FACTOR_I; |
} |
|
|
431,10 → 435,10 |
/************************************************************************/ |
void MotorRegler(void) |
{ |
int16_t MotorValue, pd_ergebnis, h, tmp_int; |
int16_t YawMixingFraction, ThrustMixingFraction; |
int16_t MotorValue, pd_result, h, tmp_int; |
int16_t YawMixFraction, ThrustMixFraction; |
static int32_t SumPitch = 0, SumRoll = 0; |
static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
static int32_t SetPointYaw = 0; |
static int32_t IntegralErrorPitch = 0; |
static int32_t IntegralErrorRoll = 0; |
static uint16_t RcLostTimer; |
442,7 → 446,7 |
static uint16_t Modell_Is_Flying = 0; |
static uint8_t EmergencyLanding = 0; |
static uint8_t HightControlActive = 0; |
static int16_t hoehenregler = 0; |
static int16_t HightControlThrust = 0; |
static int8_t TimerDebugOut = 0; |
static int8_t StoreNewCompassCourse = 0; |
static int32_t CorrectionPitch, CorrectionRoll; |
453,8 → 457,8 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// determine thrust value |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
ThrustMixingFraction = StickThrust; |
if(ThrustMixingFraction < 0) ThrustMixingFraction = 0; |
ThrustMixFraction = StickThrust; |
if(ThrustMixFraction < 0) ThrustMixFraction = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
482,7 → 486,7 |
ROT_ON; // set red led |
if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
ThrustMixingFraction = ParamSet.EmergencyThrust; // set emergency thrust |
ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust |
EmergencyLanding = 1; // enable emergency landing |
// set neutral rc inputs |
PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
502,16 → 506,16 |
EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
// reset emergency timer |
RcLostTimer = ParamSet.EmergencyThrustDuration * 50; |
if(ThrustMixingFraction > 40) |
if(ThrustMixFraction > 40) |
{ |
if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
} |
if((Modell_Is_Flying < 200) || (ThrustMixingFraction < 40)) |
if((Modell_Is_Flying < 200) || (ThrustMixFraction < 40)) |
{ |
SumPitch = 0; |
SumRoll = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
Reading_IntegralGyroYaw = 0; |
Reading_IntegralGyroYaw2 = 0; |
} |
// if motors are off and the thrust stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0) |
594,12 → 598,12 |
Modell_Is_Flying = 1; |
MotorsOn = 1; |
SetPointYaw = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
Reading_IntegralPitch = 0; |
Reading_IntegralRoll = 0; |
Reading_IntegralPitch2 = IntegralPitch; |
Reading_IntegralRoll2 = IntegralRoll; |
Reading_IntegralGyroYaw = 0; |
Reading_IntegralGyroYaw2 = 0; |
Reading_IntegralGyroPitch = 0; |
Reading_IntegralGyroRoll = 0; |
Reading_IntegralGyroPitch2 = IntegralPitch; |
Reading_IntegralGyroRoll2 = IntegralRoll; |
SumPitch = 0; |
SumRoll = 0; |
} |
639,7 → 643,7 |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers |
|
// update max stick positions for pitch and roll |
// update max stick positions for pitch, roll and yaw |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); |
else MaxStickPitch--; |
646,6 → 650,9 |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
else MaxStickRoll--; |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw) |
MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]); |
else MaxStickYaw--; |
|
// update gyro control loop factors |
|
739,7 → 746,7 |
if(Looping_Roll) BeepTime = 100; |
if(Looping_Roll || Looping_Pitch) |
{ |
if(ThrustMixingFraction > ParamSet.LoopThrustLimit) ThrustMixingFraction = ParamSet.LoopThrustLimit; |
if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
757,6 → 764,7 |
Looping_Pitch = 0; |
MaxStickPitch = 0; |
MaxStickRoll = 0; |
MaxStickYaw = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
763,24 → 771,26 |
// Trim Gyro-Integrals to ACC-Signals |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
#define ABGLEICH_ANZAHL 256L |
|
MeanIntegralPitch += IntegralPitch; // sum for averaging |
#define BALANCE_NUMBER 256L |
// sum for averaging |
MeanIntegralPitch += IntegralPitch; |
MeanIntegralRoll += IntegralRoll; |
MeanIntegralPitch2 += IntegralPitch2; |
MeanIntegralRoll2 += IntegralRoll2; |
|
if(Looping_Pitch || Looping_Roll) // if looping in any direction |
{ |
// reset averaging for acc and gyro integral as well as gyro integral acc correction |
MeasurementCounter = 0; |
|
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
|
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeanIntegralPitch2 = 0; |
MeanIntegralRoll2 = 0; |
Reading_IntegralPitch2 = Reading_IntegralPitch; |
Reading_IntegralRoll2 = Reading_IntegralRoll; |
ZaehlMessungen = 0; |
|
Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch; |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
|
AttitudeCorrectionPitch = 0; |
AttitudeCorrectionRoll = 0; |
} |
789,413 → 799,435 |
if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
{ |
int32_t tmp_long, tmp_long2; |
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long /= 16; |
// determine the deviation of gyro integral from averaged acceleration sensor |
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
tmp_long /= 16; |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long2 /= 16; |
|
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
if(MaxStickYaw > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
|
#define BALANCE 32 |
// limit correction |
if(tmp_long > BALANCE) tmp_long = BALANCE; |
if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE; |
|
Reading_IntegralPitch -= tmp_long; |
Reading_IntegralRoll -= tmp_long2; |
// correct current readings |
Reading_IntegralGyroPitch -= tmp_long; |
Reading_IntegralGyroRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int cnt = 0; |
static char last_n_p, last_n_n, last_r_p, last_r_n; |
static long MeanIntegralPitch_old, MeanIntegralRoll_old; |
|
if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
{ |
static int cnt = 0; |
static char last_n_p,last_n_n,last_r_p,last_r_n; |
static long MeanIntegralPitch_old,MeanIntegralRoll_old; |
if(!Looping_Pitch && !Looping_Roll) |
{ |
MeanIntegralPitch /= ABGLEICH_ANZAHL; |
MeanIntegralRoll /= ABGLEICH_ANZAHL; |
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / ABGLEICH_ANZAHL; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
#define MAX_I 0//(Poti2/10) |
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralErrorPitch = (long)(MeanIntegralPitch - (long)IntegralAccPitch); |
CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralErrorRoll = (long)(MeanIntegralRoll - (long)IntegralAccRoll); |
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim; |
// if not lopping in any direction (this should be alwais the case, |
// because the Measurement counter is reset to 0 if looping in any direction is active.) |
if(!Looping_Pitch && !Looping_Roll) |
{ |
// Calculate mean value of the gyro integrals |
MeanIntegralPitch /= BALANCE_NUMBER; |
MeanIntegralRoll /= BALANCE_NUMBER; |
|
AttitudeCorrectionPitch = CorrectionPitch / ABGLEICH_ANZAHL; |
AttitudeCorrectionRoll = CorrectionRoll / ABGLEICH_ANZAHL; |
// Calculate mean of the acceleration values |
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER; |
IntegralAccZ = IntegralAccZ / BALANCE_NUMBER; |
|
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) |
{ |
AttitudeCorrectionPitch /= 2; |
AttitudeCorrectionPitch /= 2; |
} |
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch); |
CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim; |
AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll); |
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim; |
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralPitch2 /= ABGLEICH_ANZAHL; |
MeanIntegralRoll2 /= ABGLEICH_ANZAHL; |
tmp_long = IntegralPitch2 - IntegralPitch; |
tmp_long2 = IntegralRoll2 - IntegralRoll; |
//DebugOut.Analog[25] = MeanIntegralRoll2 / 26; |
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25)) |
{ |
AttitudeCorrectionPitch /= 2; |
AttitudeCorrectionRoll /= 2; |
} |
|
IntegralErrorPitch = tmp_long; |
IntegralErrorRoll = tmp_long2; |
Reading_IntegralPitch2 -= IntegralErrorPitch; |
Reading_IntegralRoll2 -= IntegralErrorRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor) |
IntegralErrorPitch = IntegralPitch2 - IntegralPitch; |
Reading_IntegralGyroPitch2 -= IntegralErrorPitch; |
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor) |
IntegralErrorRoll = IntegralRoll2 - IntegralRoll; |
Reading_IntegralGyroRoll2 -= IntegralErrorRoll; |
|
// IntegralErrorPitch = (IntegralErrorPitch * 1 + tmp_long) / 2; |
// IntegralErrorRoll = (IntegralErrorRoll * 1 + tmp_long2) / 2; |
|
DebugOut.Analog[17] = IntegralAccPitch / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
DebugOut.Analog[19] = IntegralErrorPitch;// / 26; |
DebugOut.Analog[20] = IntegralErrorRoll;// / 26; |
DebugOut.Analog[21] = MeanIntegralPitch / 26; |
DebugOut.Analog[22] = MeanIntegralRoll / 26; |
//DebugOut.Analog[28] = CorrectionPitch; |
DebugOut.Analog[29] = CorrectionRoll; |
DebugOut.Analog[30] = AttitudeCorrectionRoll * 10; |
|
DebugOut.Analog[17] = IntegralAccPitch / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
DebugOut.Analog[19] = IntegralErrorPitch;// / 26; |
DebugOut.Analog[20] = IntegralErrorRoll;// / 26; |
DebugOut.Analog[21] = MeanIntegralPitch / 26; |
DebugOut.Analog[22] = MeanIntegralRoll / 26; |
//DebugOut.Analog[28] = CorrectionPitch; |
DebugOut.Analog[29] = CorrectionRoll; |
DebugOut.Analog[30] = AttitudeCorrectionRoll * 10; |
#define ERROR_LIMIT (BALANCE_NUMBER * 4) |
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16) |
#define MOVEMENT_LIMIT 20000 |
// Pitch +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorPitch) / 4096; |
CorrectionPitch = 0; |
if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT) |
{ |
if(IntegralErrorPitch > ERROR_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2; |
CorrectionPitch = IntegralErrorPitch / 8; |
if(CorrectionPitch > 5000) CorrectionPitch = 5000; |
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER; |
} |
else last_n_p = 1; |
} |
else last_n_p = 0; |
if(IntegralErrorPitch < -ERROR_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2; |
CorrectionPitch = IntegralErrorPitch / 8; |
if(CorrectionPitch < -5000) CorrectionPitch = -5000; |
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER; |
} |
else last_n_n = 1; |
} |
else last_n_n = 0; |
} |
else cnt = 0; |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
// correct Gyro Offsets |
if(IntegralErrorPitch > ERROR_LIMIT) AdNeutralPitch += cnt; |
if(IntegralErrorPitch < -ERROR_LIMIT) AdNeutralPitch -= cnt; |
|
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) |
#define BEWEGUNGS_LIMIT 20000 |
// Pitch +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorPitch) / 4096; |
if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < BEWEGUNGS_LIMIT) |
{ |
if(IntegralErrorPitch > FEHLER_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralErrorPitch) / FEHLER_LIMIT2; |
CorrectionPitch = IntegralErrorPitch / 8; |
if(CorrectionPitch > 5000) CorrectionPitch = 5000; |
AttitudeCorrectionPitch += CorrectionPitch / ABGLEICH_ANZAHL; |
} |
else last_n_p = 1; |
} else last_n_p = 0; |
if(IntegralErrorPitch < -FEHLER_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralErrorPitch) / FEHLER_LIMIT2; |
CorrectionPitch = IntegralErrorPitch / 8; |
if(CorrectionPitch < -5000) CorrectionPitch = -5000; |
AttitudeCorrectionPitch += CorrectionPitch / ABGLEICH_ANZAHL; |
} |
else last_n_n = 1; |
} else last_n_n = 0; |
} else cnt = 0; |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(IntegralErrorPitch > FEHLER_LIMIT) AdNeutralPitch += cnt; |
if(IntegralErrorPitch < -FEHLER_LIMIT) AdNeutralPitch -= cnt; |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorPitch) / 4096; |
CorrectionRoll = 0; |
if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) |
{ |
if(IntegralErrorRoll > ERROR_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll > 5000) CorrectionRoll = 5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_p = 1; |
} |
else last_r_p = 0; |
if(IntegralErrorRoll < -ERROR_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll < -5000) CorrectionRoll = -5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_n = 1; |
} |
else last_r_n = 0; |
} |
else cnt = 0; |
// correct Gyro Offsets |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt; |
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt; |
|
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorPitch) / 4096; |
DebugOut.Analog[27] = CorrectionRoll; |
DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch); |
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
} |
else // looping is active |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionPitch = 0; |
} |
|
CorrectionRoll = 0; |
if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < BEWEGUNGS_LIMIT) |
{ |
if(IntegralErrorRoll > FEHLER_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralErrorRoll) / FEHLER_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll > 5000) CorrectionRoll = 5000; |
AttitudeCorrectionRoll += CorrectionRoll / ABGLEICH_ANZAHL; |
} |
else last_r_p = 1; |
} else last_r_p = 0; |
if(IntegralErrorRoll < -FEHLER_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralErrorRoll) / FEHLER_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll < -5000) CorrectionRoll = -5000; |
AttitudeCorrectionRoll += CorrectionRoll / ABGLEICH_ANZAHL; |
} |
else last_r_n = 1; |
} else last_r_n = 0; |
} else |
{ |
cnt = 0; |
} |
// if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction |
if(!Gyro_I_Factor) |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionPitch = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralPitch_old = MeanIntegralPitch; |
MeanIntegralRoll_old = MeanIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// reset variables used for averaging |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeasurementCounter = 0; |
} // end of averaging |
|
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(IntegralErrorRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralErrorRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
DebugOut.Analog[27] = CorrectionRoll; |
DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch); |
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
} |
else |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionPitch = 0; |
} |
|
if(!Gyro_I_Factor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralPitch_old = MeanIntegralPitch; |
MeanIntegralRoll_old = MeanIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeanIntegralPitch2 = 0; |
MeanIntegralRoll2 = 0; |
ZaehlMessungen = 0; |
} |
//DebugOut.Analog[31] = StickRoll / (26*Gyro_I_Factor); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickYaw) > 20) // war 35 |
{ |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1; |
} |
tmp_int = (long) ParamSet.Yaw_P * ((long)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
SetPointYaw = tmp_int; |
Reading_IntegralYaw -= tmp_int; |
if(Reading_IntegralYaw > 50000) Reading_IntegralYaw = 50000; // begrenzen |
if(Reading_IntegralYaw <-50000) Reading_IntegralYaw =-50000; |
if(MaxStickYaw > 20) // yaw stick is activated |
{ // if not fixed compass course is set update compass course |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1; |
} |
// exponential stick sensitivity in yawring rate |
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
SetPointYaw = tmp_int; |
Reading_IntegralGyroYaw -= tmp_int; |
// limit the effect |
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000; |
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) |
{ |
int w,v; |
static uint8_t updCompass = 0; |
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) |
{ |
int16_t w,v; |
static uint8_t updCompass = 0; |
|
if (!updCompass--) // Aufruf mit ~10 Hz |
{ |
CompassHeading = MM3_heading(); // get current compass reading |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
updCompass = 50; |
} |
if (!updCompass--) |
{ |
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
// get current compass heading (angule between MK head and magnetic north) |
CompassHeading = MM3_heading(); |
// calculate OffCourse (angular deviation from heading to course) |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
|
w = abs(IntegralPitch /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
if(w < 35 && StoreNewCompassCourse) |
{ |
CompassCourse = CompassHeading; |
StoreNewCompassCourse = 0; |
} |
w = (w * FCParam.CompassYawEffect) / 64; // auf die Wirkung normieren |
w = FCParam.CompassYawEffect - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
Reading_IntegralYaw -= (CompassOffCourse * w) / 32; // nach Kompass ausrichten |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
} |
|
// reduce compass effect with increasing declination |
w = abs(IntegralPitch / 512); |
v = abs(IntegralRoll / 512); |
if(v > w) w = v; // get maximum declination |
// if declination is small enough update compass course if neccessary |
if(w < 35 && StoreNewCompassCourse) |
{ |
CompassCourse = CompassHeading; |
StoreNewCompassCourse = 0; |
} |
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination |
if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
{ |
Reading_IntegralGyroYaw -= (CompassOffCourse * w) / 32; |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; |
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
DebugOut.Analog[2] = Mean_AccPitch; |
DebugOut.Analog[3] = Mean_AccRoll; |
DebugOut.Analog[4] = ReadingYaw; |
DebugOut.Analog[5] = ReadingHight; |
DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[16] = Mean_AccTop; |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
DebugOut.Analog[2] = Mean_AccPitch; |
DebugOut.Analog[3] = Mean_AccRoll; |
DebugOut.Analog[4] = Reading_GyroYaw; |
DebugOut.Analog[5] = ReadingHight; |
DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[16] = Mean_AccTop; |
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |
DebugOut.Analog[19] = motor_rx[3]; |
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
DebugOut.Analog[20] /= 14; |
DebugOut.Analog[21] = motor_rx[4]; |
DebugOut.Analog[22] = motor_rx[5]; |
DebugOut.Analog[23] = motor_rx[6]; |
DebugOut.Analog[24] = motor_rx[7]; |
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
*/ |
// DebugOut.Analog[9] = ReadingPitch; |
// DebugOut.Analog[9] = SetPointHight; |
// DebugOut.Analog[10] = Reading_IntegralYaw / 128; |
// DebugOut.Analog[11] = CompassCourse; |
// DebugOut.Analog[10] = FCParam.Gyro_I; |
// DebugOut.Analog[10] = ParamSet.Gyro_I; |
// DebugOut.Analog[9] = CompassOffCourse; |
// DebugOut.Analog[10] = ThrustMixingFraction; |
// DebugOut.Analog[3] = HightD * 32; |
// DebugOut.Analog[4] = hoehenregler; |
} |
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |
DebugOut.Analog[19] = motor_rx[3]; |
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
DebugOut.Analog[20] /= 14; |
DebugOut.Analog[21] = motor_rx[4]; |
DebugOut.Analog[22] = motor_rx[5]; |
DebugOut.Analog[23] = motor_rx[6]; |
DebugOut.Analog[24] = motor_rx[7]; |
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
|
DebugOut.Analog[9] = Reading_GyroPitch; |
DebugOut.Analog[9] = SetPointHight; |
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128; |
DebugOut.Analog[11] = CompassCourse; |
DebugOut.Analog[10] = FCParam.Gyro_I; |
DebugOut.Analog[10] = ParamSet.Gyro_I; |
DebugOut.Analog[9] = CompassOffCourse; |
DebugOut.Analog[10] = ThrustMixFraction; |
DebugOut.Analog[3] = HightD * 32; |
DebugOut.Analog[4] = HightControlThrust; |
*/ |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[26] = ReadingPitch; |
//DebugOut.Analog[28] = ReadingRoll; |
|
if(Looping_Pitch) ReadingPitch = ReadingPitch * Gyro_P_Factor; |
else ReadingPitch = IntegralPitch * Gyro_I_Factor + ReadingPitch * Gyro_P_Factor; |
if(Looping_Roll) ReadingRoll = ReadingRoll * Gyro_P_Factor; |
else ReadingRoll = IntegralRoll * Gyro_I_Factor + ReadingRoll * Gyro_P_Factor; |
ReadingYaw = ReadingYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor; |
else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor; |
if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor; |
else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor; |
Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
|
DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor; |
DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor); |
DebugOut.Analog[28] = ReadingRoll; |
DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor; |
DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor); |
DebugOut.Analog[28] = Reading_GyroRoll; |
|
// Maximalwerte abfangen |
#define MAX_SENSOR 2048 |
if(ReadingPitch > MAX_SENSOR) ReadingPitch = MAX_SENSOR; |
if(ReadingPitch < -MAX_SENSOR) ReadingPitch = -MAX_SENSOR; |
if(ReadingRoll > MAX_SENSOR) ReadingRoll = MAX_SENSOR; |
if(ReadingRoll < -MAX_SENSOR) ReadingRoll = -MAX_SENSOR; |
if(ReadingYaw > MAX_SENSOR) ReadingYaw = MAX_SENSOR; |
if(ReadingYaw < -MAX_SENSOR) ReadingYaw = -MAX_SENSOR; |
// limit control feedback |
#define MAX_SENSOR 2048 |
if(Reading_GyroPitch > MAX_SENSOR) Reading_GyroPitch = MAX_SENSOR; |
if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR; |
if(Reading_GyroRoll > MAX_SENSOR) Reading_GyroRoll = MAX_SENSOR; |
if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR; |
if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Höhenregelung |
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
// Hight Control |
// The higth control algorithm reduces the thrust but does not increas the thrust. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//OCR0B = 180 - (Poti1 + 120) / 4; |
//DruckOffsetSetting = OCR0B; |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung |
{ |
int tmp_int; |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) // Regler wird über Schalter gesteuert |
{ |
if(FCParam.MaxHight < 50) |
{ |
SetPointHight = ReadingHight - 20; // FCParam.MaxHight ist der PPM-Wert des Schalters |
HightControlActive = 0; |
} |
else |
HightControlActive = 1; |
} |
else |
{ |
SetPointHight = ((int) ExternHightValue + (int) FCParam.MaxHight) * (int)ParamSet.Hight_Gain - 20; |
HightControlActive = 1; |
} |
|
if(EmergencyLanding) SetPointHight = 0; |
h = ReadingHight; |
if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
{ h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
h = ThrustMixingFraction - h; // vom Gas abziehen |
h -= (HightD * FCParam.AirPressure_D)/8; // D-Anteil |
tmp_int = ((Reading_Integral_Top / 512) * (signed long) FCParam.Hight_ACC_Effect) / 32; |
if(tmp_int > 50) tmp_int = 50; |
else if(tmp_int < -50) tmp_int = -50; |
h -= tmp_int; |
hoehenregler = (hoehenregler*15 + h) / 16; |
if(hoehenregler < ParamSet.Hight_MinThrust) // nicht unter MIN |
{ |
if(ThrustMixingFraction >= ParamSet.Hight_MinThrust) hoehenregler = ParamSet.Hight_MinThrust; |
if(ThrustMixingFraction < ParamSet.Hight_MinThrust) hoehenregler = ThrustMixingFraction; |
} |
if(hoehenregler > ThrustMixingFraction) hoehenregler = ThrustMixingFraction; // nicht mehr als Gas |
ThrustMixingFraction = hoehenregler; |
} |
} |
if(ThrustMixingFraction > ParamSet.Trust_Max - 20) ThrustMixingFraction = ParamSet.Trust_Max - 20; |
// If hight control is activated and no emergency landing is activre |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
{ |
int tmp_int; |
// if hight control is activated by an rc channel |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
if(FCParam.MaxHight < 50) |
{ |
SetPointHight = ReadingHight - 20; // update SetPoint with current reading |
HightControlActive = 0; // disable hight control |
} |
else HightControlActive = 1; // enable hight control |
} |
else // no switchable hight control |
{ |
SetPointHight = ((int16_t) ExternHightValue + (int16_t) FCParam.MaxHight) * (int16_t)ParamSet.Hight_Gain - 20; |
HightControlActive = 1; |
} |
// get current hight |
h = ReadingHight; |
// if current hight is above the setpoint reduce thrust |
if((h > SetPointHight) && HightControlActive) |
{ |
// hight difference -> P control part |
h = ((h - SetPointHight) * (int16_t) FCParam.Hight_P) / 16; |
h = ThrustMixFraction - h; // reduce gas |
// higth gradient --> D control part |
h -= (HightD * FCParam.Hight_D) / 8; // D control part |
// acceleration sensor effect |
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Hight_ACC_Effect) / 32; |
if(tmp_int > 50) tmp_int = 50; |
if(tmp_int < -50) tmp_int = -50; |
h -= tmp_int; |
// update hight control thrust |
HightControlThrust = (HightControlThrust*15 + h) / 16; |
// limit thrust reduction |
if(HightControlThrust < ParamSet.Hight_MinThrust) |
{ |
if(ThrustMixFraction >= ParamSet.Hight_MinThrust) HightControlThrust = ParamSet.Hight_MinThrust; |
// allows landing also if thrust stick is reduced below min thrust on hight control |
if(ThrustMixFraction < ParamSet.Hight_MinThrust) HightControlThrust = ThrustMixFraction; |
} |
// limit thrust to stick setting |
if(HightControlThrust > ThrustMixFraction) HightControlThrust = ThrustMixFraction; |
ThrustMixFraction = HightControlThrust; |
} |
} |
// limit thrust to parameter setting |
if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// + Mixer and PI-Controller |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = ThrustMixingFraction; |
DebugOut.Analog[7] = ThrustMixFraction; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gier-Anteil |
// Yaw-Fraction |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define MUL_G 1.0 |
YawMixingFraction = ReadingYaw - SetPointYaw; // Regler für Gier |
// YawMixingFraction = 0; |
YawMixFraction = Reading_GyroYaw - SetPointYaw; // yaw controller |
|
if(YawMixingFraction > (ThrustMixingFraction / 2)) YawMixingFraction = ThrustMixingFraction / 2; |
if(YawMixingFraction < -(ThrustMixingFraction / 2)) YawMixingFraction = -(ThrustMixingFraction / 2); |
if(YawMixingFraction > ((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = ((ParamSet.Trust_Max - ThrustMixingFraction)); |
if(YawMixingFraction < -((ParamSet.Trust_Max - ThrustMixingFraction))) YawMixingFraction = -((ParamSet.Trust_Max - ThrustMixingFraction)); |
|
if(ThrustMixingFraction < 20) YawMixingFraction = 0; |
// limit YawMixFraction |
if(YawMixFraction > (ThrustMixFraction / 2)) YawMixFraction = ThrustMixFraction / 2; |
if(YawMixFraction < -(ThrustMixFraction / 2)) YawMixFraction = -(ThrustMixFraction / 2); |
if(YawMixFraction > ((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = ((ParamSet.Trust_Max - ThrustMixFraction)); |
if(YawMixFraction < -((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = -((ParamSet.Trust_Max - ThrustMixFraction)); |
if(ThrustMixFraction < 20) YawMixFraction = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Pitch-Achse |
// Pitch-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
else SumPitch += DiffPitch; // I-Anteil bei HH |
DiffPitch = Reading_GyroPitch - (StickPitch - GPS_Pitch); // get difference |
if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-part for attitude control |
else SumPitch += DiffPitch; // I-part for head holding |
if(SumPitch > 16000) SumPitch = 16000; |
if(SumPitch < -16000) SumPitch = -16000; |
pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
// Motor Vorn |
tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
pd_result = DiffPitch + Ki * SumPitch; // PI-controller for pitch |
|
MotorValue = ThrustMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
// Motor Front |
MotorValue = ThrustMixFraction + pd_result + YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Front = MotorValue; |
// Motor Heck |
MotorValue = ThrustMixingFraction - pd_ergebnis + YawMixingFraction; |
|
// Motor Rear |
MotorValue = ThrustMixFraction - pd_result + YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Rear = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
else SumRoll += DiffRoll; // I-Anteil bei HH |
DiffRoll = Reading_GyroRoll - (StickRoll - GPS_Roll); // get difference |
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll); // I-part for attitude control |
else SumRoll += DiffRoll; // I-part for head holding |
if(SumRoll > 16000) SumRoll = 16000; |
if(SumRoll < -16000) SumRoll = -16000; |
pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
tmp_int = (long)((long)FCParam.DynamicStability * (long)(ThrustMixingFraction + abs(YawMixingFraction)/2)) / 64; |
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
// Motor Links |
MotorValue = ThrustMixingFraction + pd_ergebnis - YawMixingFraction; |
#define GRENZE Poti1 |
pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
// Motor Left |
MotorValue = ThrustMixFraction + pd_result - YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Left = MotorValue; |
// Motor Rechts |
MotorValue = ThrustMixingFraction - pd_ergebnis - YawMixingFraction; |
|
// Motor Right |
MotorValue = ThrustMixFraction - pd_result - YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Right = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |
|