Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 710 → Rev 711

/branches/V0.68d Code Redesign killagreg/analog.c
20,14 → 20,13
volatile int16_t UBat = 100;
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0;
volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0;
volatile uint8_t messanzahl_AccHoch = 0;
volatile int32_t AirPressure = 32000;
volatile int16_t StartAirPressure;
volatile uint16_t ReadingAirPressure = 1023;
uint8_t DruckOffsetSetting;
uint8_t PressureSensorOffset;
volatile int16_t HightD = 0;
volatile int16_t tmpAirPressure;
volatile uint16_t ZaehlMessungen = 0;
volatile uint16_t MeasurementCounter = 0;
 
/*****************************************************/
/* Initialize Analog Digital Converter */
75,7 → 74,7
if(ReadingAirPressure < 900) break;
}
SetParamByte(PID_LAST_OFFSET, off);
DruckOffsetSetting = off;
PressureSensorOffset = off;
Delay_ms_Mess(300);
}
 
96,7 → 95,7
case 0:
yaw1 = ADC; // get Gyro Yaw Voltage 1st sample
adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
ZaehlMessungen++; // increment total measurement counter
MeasurementCounter++; // increment total measurement counter
break;
case 1:
roll1 = ADC; // get Gyro Roll Voltage 1st sample
151,7 → 150,6
{
if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
}
messanzahl_AccHoch = 1;
Current_AccZ = ADC;
Reading_Integral_Top += AdValueAccTop; // Integrieren
Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
/branches/V0.68d Code Redesign killagreg/analog.h
9,8 → 9,8
extern volatile int16_t Current_Pitch, Current_Roll, Current_Yaw;
extern volatile int16_t Current_AccX, Current_AccY, Current_AccZ;
extern volatile int32_t AirPressure;
extern volatile uint16_t ZaehlMessungen;
extern uint8_t DruckOffsetSetting;
extern volatile uint16_t MeasurementCounter;
extern uint8_t PressureSensorOffset;
extern volatile int16_t HightD;
extern volatile uint16_t ReadingAirPressure;
extern volatile int16_t StartAirPressure;
/branches/V0.68d Code Redesign killagreg/eeprom.c
41,7 → 41,7
ParamSet.Hight_MinThrust = 30;
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Hight_P = 10; // Wert : 0-32
ParamSet.AirPressure_D = 30; // Wert : 0-250
ParamSet.Hight_D = 30; // Wert : 0-250
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250
ParamSet.Hight_Gain = 4; // Wert : 0-50
ParamSet.Stick_P = 4; //2 // Wert : 1-6
104,7 → 104,7
ParamSet.Hight_MinThrust = 30;
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Hight_P = 10; // Wert : 0-32
ParamSet.AirPressure_D = 30; // Wert : 0-250
ParamSet.Hight_D = 30; // Wert : 0-250
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250
ParamSet.Hight_Gain = 3; // Wert : 0-50
ParamSet.Stick_P = 3; //2 // Wert : 1-6
167,7 → 167,7
ParamSet.Hight_MinThrust = 30;
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Hight_P = 10; // Wert : 0-32
ParamSet.AirPressure_D = 30; // Wert : 0-250
ParamSet.Hight_D = 30; // Wert : 0-250
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250
ParamSet.Hight_Gain = 2; // Wert : 0-50
ParamSet.Stick_P = 2; //2 // Wert : 1-6
/branches/V0.68d Code Redesign killagreg/eeprom.h
55,7 → 55,7
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t GlobalConfig; // see upper defines for bitcoding
uint8_t Hight_MinThrust; // Wert : 0-100
uint8_t AirPressure_D; // Wert : 0-250
uint8_t Hight_D; // Wert : 0-250
uint8_t MaxHight; // Wert : 0-32
uint8_t Hight_P; // Wert : 0-32
uint8_t Hight_Gain; // Wert : 0-50
/branches/V0.68d Code Redesign killagreg/fc.c
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;
// +++++++++++++++++++++++++++++++++++++++++++++++
}
 
/branches/V0.68d Code Redesign killagreg/fc.h
7,7 → 7,7
 
typedef struct
{
uint8_t AirPressure_D;
uint8_t Hight_D;
uint8_t MaxHight;
uint8_t Hight_P;
uint8_t Hight_ACC_Effect;
37,7 → 37,7
 
// attitude
extern volatile int32_t IntegralPitch, IntegralRoll, IntegralYaw;
extern volatile int16_t ReadingPitch, ReadingRoll, ReadingYaw;
extern volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
 
// offsets
extern volatile int16_t AdNeutralPitch, AdNeutralRoll, AdNeutralYaw;
/branches/V0.68d Code Redesign killagreg/menu.c
94,7 → 94,7
LCD_printfxy(0,0,"Hight: %5i",ReadingHight);
LCD_printfxy(0,1,"Set Point: %5i",SetPointHight);
LCD_printfxy(0,2,"Air Press.: %5i",ReadingAirPressure);
LCD_printfxy(0,3,"Offset : %5i",DruckOffsetSetting);
LCD_printfxy(0,3,"Offset : %5i",PressureSensorOffset);
}
else
{
127,13 → 127,13
{
LCD_printfxy(0,1,"Pitch %4i (%3i)",AdValueGyrPitch - AdNeutralPitch, AdNeutralPitch);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll);
LCD_printfxy(0,3,"Yaw %4i (%3i)",ReadingYaw, AdNeutralYaw);
LCD_printfxy(0,3,"Yaw %4i (%3i)",Reading_GyroYaw, AdNeutralYaw);
}
else
{
LCD_printfxy(0,1,"Pitch %4i (%3i)",AdValueGyrPitch - AdNeutralPitch, AdNeutralPitch/2);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2);
LCD_printfxy(0,3,"Yaw %4i (%3i)",ReadingYaw, AdNeutralYaw/2);
LCD_printfxy(0,3,"Yaw %4i (%3i)",Reading_GyroYaw, AdNeutralYaw/2);
}
break;
case 6:// Acceleration Sensor Menu Item
/branches/V0.68d Code Redesign killagreg/rc.c
106,8 → 106,10
if(index < 10) // channel limit is 9 because of the frame length of 22.5 ms
{
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if((signal > 250) && (signal < 687))
{
// shift signal to zero symmetric range -154 to 159
signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
// check for stable signal
// the deviation of the current signal level from the average must be less than 6 (aprox. 1%)
114,8 → 116,8
if(abs(signal - PPM_in[index]) < 6)
{
// a good signal condition increases SenderOkay by 10
// SignalOkay is decremented every 2 ms in timer0.c
// this variable is a level for the average rate of noiseless signal
// SignalOkay is decremented every 2 ms in main.c
// this variable is a level for the average rate of a noiseless rc signal
if(SenderOkay < 200) SenderOkay += 10;
}
// calculate exponential history for signal
/branches/V0.68d Code Redesign killagreg/uart.c
68,13 → 68,13
"IntegralErrPitch",
"IntegralErrRoll ", //20
"MeanIntPitch ",
"MMeanIntRoll ",
"MeanIntRoll ",
"NeutralPitch ",
"RollOffset ",
"IntRoll*Factor ", //25
"ReadingPitch ",
"Reading_GyroPitch ",
"DirectCorrRoll ",
"ReadingRoll ",
"Reading_GyroRoll ",
"CorrectionRoll ",
"I-AttRoll ", //30
"StickRoll "