66,84 → 66,97 |
#include "twimaster.h" |
#include "mm3.h" |
|
unsigned char h,m,s; |
|
volatile unsigned int I2CTimeout = 100; |
volatile int ReadingPitch, ReadingRoll, ReadingYaw; |
volatile int AdNeutralPitch = 0,AdNeutralRoll = 0,AdNeutralYaw = 0, StartNeutralRoll = 0, StartNeutralPitch = 0; |
volatile int Mean_AccPitch, Mean_AccRoll, Mean_AccTop, NeutralAccX=0, NeutralAccY=0; |
// gyro readings |
volatile int16_t ReadingPitch, ReadingRoll, ReadingYaw; |
// gyro neutral readings |
volatile int16_t AdNeutralPitch = 0, AdNeutralRoll = 0, AdNeutralYaw = 0; |
volatile int16_t StartNeutralRoll = 0, StartNeutralPitch = 0; |
// mean accelerations |
volatile int16_t Mean_AccPitch, Mean_AccRoll, Mean_AccTop; |
|
// neutral acceleration readings |
volatile int16_t NeutralAccX=0, NeutralAccY=0; |
volatile float NeutralAccZ = 0; |
volatile long IntegralPitch = 0,IntegralPitch2 = 0; |
volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
volatile long IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
volatile long Integral_Yaw = 0; |
volatile long Reading_IntegralPitch = 0,Reading_IntegralPitch2 = 0; |
volatile long Reading_IntegralRoll = 0,Reading_IntegralRoll2 = 0; |
volatile long Reading_Integral_Yaw = 0,Reading_Integral_Yaw2 = 0; |
volatile long MeanIntegralPitch, MeanIntegralRoll, MeanIntegralPitch2, MeanIntegralRoll2; |
volatile long Reading_Integral_Top = 0; |
volatile int CompassHeading = 0; |
volatile int CompassCourse = 0; |
volatile int CompassOffCourse = 0; |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char EmergencyLanding = 0; |
unsigned char HightControlActive = 0; |
long TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
|
// attitude gyro integrals |
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; |
|
// attitude acceleration integrals |
volatile int32_t IntegralAccPitch = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
volatile int32_t Reading_Integral_Top = 0; |
|
// compass course |
volatile int16_t CompassHeading = 0; |
volatile int16_t CompassCourse = 0; |
volatile int16_t CompassOffCourse = 0; |
|
// flags |
uint8_t EmergencyLanding = 0; |
uint8_t HightControlActive = 0; |
uint8_t MotorsOn = 0; |
|
int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
|
float GyroFactor; |
float IntegralFactor; |
volatile int DiffPitch,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Front, Motor_Rear, Motor_Right, Motor_Left, Count; |
unsigned char MotorValue[5]; |
|
volatile int16_t DiffPitch, DiffRoll; |
|
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
|
// setpoints for motors |
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
|
// stick values derived by rc channels readings |
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
char MotorsOn = 0; |
int ReadingHight = 0; |
int SetPointHight = 0; |
int AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0; |
float Ki = FAKTOR_I; |
unsigned char Looping_Pitch = 0, Looping_Roll = 0; |
unsigned char Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0; |
// stick values derived by uart inputs |
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
|
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
unsigned char Parameter_CompassYawEffect = 64; // Wert : 0-250 |
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
unsigned char Parameter_I_Factor = 10; // Wert : 1-20 |
unsigned char Parameter_UserParam1 = 0; |
unsigned char Parameter_UserParam2 = 0; |
unsigned char Parameter_UserParam3 = 0; |
unsigned char Parameter_UserParam4 = 0; |
unsigned char Parameter_UserParam5 = 0; |
unsigned char Parameter_UserParam6 = 0; |
unsigned char Parameter_UserParam7 = 0; |
unsigned char Parameter_UserParam8 = 0; |
unsigned char Parameter_ServoPitchControl = 100; |
unsigned char Parameter_LoopGasLimit = 70; |
unsigned char Parameter_AchsKopplung1 = 0; |
unsigned char Parameter_AchsGegenKopplung1 = 0; |
unsigned char Parameter_DynamicStability = 100; |
|
signed int ExternStickPitch = 0, ExternStickRoll = 0,ExternStickYaw = 0, ExternHightValue = -20; |
int MaxStickPitch = 0, MaxStickRoll = 0; |
|
void Piep(unsigned char Anzahl) |
|
int16_t ReadingHight = 0; |
int16_t SetPointHight = 0; |
|
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0; |
|
float Ki = FACTOR_I; |
|
uint8_t Looping_Pitch = 0, Looping_Roll = 0; |
uint8_t Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0; |
|
|
fc_param_t FCParam = {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100}; |
|
|
/************************************************************************/ |
/* Creates numbeeps beeps at the speaker */ |
/************************************************************************/ |
void Beep(uint8_t numbeeps) |
{ |
while(Anzahl--) |
while(numbeeps--) |
{ |
if(MotorsOn) return; //auf keinen Fall im Flug! |
BeepTime = 100; |
Delay_ms(250); |
BeepTime = 100; // 0.1 second |
Delay_ms(250); // blocks 250 ms as pause to next beep, |
// this will block the flight control loop, |
// therefore do not use this funktion if motors are running |
} |
} |
|
//############################################################################ |
// Nullwerte ermitteln |
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(void) |
//############################################################################ |
{ |
NeutralAccX = 0; |
NeutralAccY = 0; |
151,16 → 164,15 |
AdNeutralPitch = 0; |
AdNeutralRoll = 0; |
AdNeutralYaw = 0; |
Parameter_AchsKopplung1 = 0; |
Parameter_AchsGegenKopplung1 = 0; |
FCParam.AchsKopplung1 = 0; |
FCParam.AchsGegenKopplung1 = 0; |
CalibMean(); |
Delay_ms_Mess(100); |
CalibMean(); |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
{ |
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
} |
|
AdNeutralPitch= AdValueGyrPitch; |
AdNeutralRoll= AdValueGyrRoll; |
AdNeutralYaw= AdValueGyrYaw; |
173,70 → 185,74 |
NeutralAccZ = Current_AccZ; |
} |
else |
{ // why not GetParamWord()? |
NeutralAccX = (int)GetParamByte(PID_ACC_PITCH) * 256 + (int)GetParamByte(PID_ACC_PITCH+1); |
NeutralAccY = (int)GetParamByte(PID_ACC_ROLL) * 256 + (int)GetParamByte(PID_ACC_ROLL+1); |
NeutralAccZ = (int)GetParamByte(PID_ACC_Z) * 256 + (int)GetParamByte(PID_ACC_Z+1); |
{ |
NeutralAccX = (int16_t)GetParamWord(PID_ACC_PITCH); |
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_Integral_Yaw = 0; |
Reading_IntegralYaw = 0; |
ReadingPitch = 0; |
ReadingRoll = 0; |
ReadingYaw = 0; |
StartAirPressure = AirPressure; |
HoeheD = 0; |
HightD = 0; |
Reading_Integral_Top = 0; |
CompassCourse = CompassHeading; |
GPS_Neutral(); |
BeepTime = 50; |
TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
ExternHightValue = 0; |
} |
|
//############################################################################ |
// Bearbeitet die Messwerte |
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
//############################################################################ |
{ |
static int32_t tmpl,tmpl2; |
ReadingYaw = (int16_t) AdNeutralYaw - AdValueGyrYaw; |
ReadingRoll = (int16_t) AdValueGyrRoll - AdNeutralRoll; |
ReadingPitch = (int16_t) AdValueGyrPitch - AdNeutralPitch; |
|
//DebugOut.Analog[26] = ReadingPitch; |
// Get offset corrected gyro readings |
ReadingYaw = AdNeutralYaw - AdValueGyrYaw; |
ReadingRoll = AdValueGyrRoll - AdNeutralRoll; |
ReadingPitch = AdValueGyrPitch - AdNeutralPitch; |
|
DebugOut.Analog[26] = ReadingPitch; |
DebugOut.Analog[28] = ReadingRoll; |
|
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mean_AccPitch = ((long)Mean_AccPitch * 1 + ((ACC_AMPLIFY * (long)AdValueAccPitch))) / 2L; |
Mean_AccRoll = ((long)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdValueAccRoll))) / 2L; |
Mean_AccTop = ((long)Mean_AccTop * 1 + ((long)AdValueAccTop)) / 2L; |
// Acceleration Sensor |
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; |
|
IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch; |
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
IntegralAccZ += Current_AccZ - NeutralAccZ; |
// Yaw ++++++++++++++++++++++++++++++++++++++++++++++++ |
Reading_Integral_Yaw += ReadingYaw; |
Reading_Integral_Yaw2 += ReadingYaw; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
|
// Yaw |
Reading_IntegralYaw += ReadingYaw; |
Reading_IntegralYaw2 += ReadingYaw; |
|
// Coupling fraction |
if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
tmpl = Reading_IntegralPitch / 4096L; |
tmpl *= ReadingYaw; |
tmpl *= Parameter_AchsKopplung1; //125 |
tmpl *= FCParam.AchsKopplung1; //125 |
tmpl /= 2048L; |
tmpl2 = Reading_IntegralRoll / 4096L; |
tmpl2 *= ReadingYaw; |
tmpl2 *= Parameter_AchsKopplung1; |
tmpl2 *= FCParam.AchsKopplung1; |
tmpl2 /= 2048L; |
} |
else tmpl = tmpl2 = 0; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll |
ReadingRoll += tmpl; |
ReadingRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109 |
Reading_IntegralRoll2 += ReadingRoll; |
Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
if(Reading_IntegralRoll > TurnOver180Roll) |
261,9 → 277,9 |
if(AdValueGyrRoll > 2020) ReadingRoll = +1000; |
if(AdValueGyrRoll > 2034) ReadingRoll = +2000; |
} |
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Pitch |
ReadingPitch -= tmpl2; |
ReadingPitch -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L; |
Reading_IntegralPitch2 += ReadingPitch; |
Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
if(Reading_IntegralPitch > TurnOver180Pitch) |
288,12 → 304,11 |
if(AdValueGyrPitch > 2020) ReadingPitch = +1000; |
if(AdValueGyrPitch > 2034) ReadingPitch = +2000; |
} |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
|
// start ADC |
ADC_Enable(); |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
|
Integral_Yaw = Reading_Integral_Yaw; |
IntegralYaw = Reading_IntegralYaw; |
IntegralPitch = Reading_IntegralPitch; |
IntegralRoll = Reading_IntegralRoll; |
IntegralPitch2 = Reading_IntegralPitch2; |
306,10 → 321,12 |
if(ReadingRoll > 200) ReadingRoll += 4 * (ReadingRoll - 200); |
else if(ReadingRoll < -200) ReadingRoll += 4 * (ReadingRoll + 200); |
} |
//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--; |
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--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
//limit poti values |
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
316,38 → 333,42 |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
} |
|
//############################################################################ |
// Messwerte beim Ermitteln der Nullage |
/************************************************************************/ |
/* Averaging Measurement Readings for Calibration */ |
/************************************************************************/ |
void CalibMean(void) |
//############################################################################ |
{ |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
// stop ADC to avoid changing values during calculation |
ADC_Disable(); |
|
ReadingPitch = AdValueGyrPitch; |
ReadingRoll = AdValueGyrRoll; |
ReadingYaw = AdValueGyrYaw; |
Mean_AccPitch = ACC_AMPLIFY * (long)AdValueAccPitch; |
Mean_AccRoll = ACC_AMPLIFY * (long)AdValueAccRoll; |
Mean_AccTop = (long)AdValueAccTop; |
// ADC einschalten |
|
Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch; |
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--; |
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--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
//limit poti values |
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
|
TurnOver180Pitch = (long) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (long) ParamSet.AngleTurnOverRoll * 2500L; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
} |
|
//############################################################################ |
// Senden der Motorwerte per I2C-Bus |
/************************************************************************/ |
/* Transmit Motor Data via I2C */ |
/************************************************************************/ |
void SendMotorData(void) |
//############################################################################ |
{ |
if(MOTOR_OFF || !MotorsOn) |
{ |
374,38 → 395,36 |
|
|
|
//############################################################################ |
// Trägt ggf. das Poti als Parameter ein |
void ParameterZuordnung(void) |
//############################################################################ |
/************************************************************************/ |
/* Maps the parameter to poti values */ |
/************************************************************************/ |
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(Parameter_MaxHoehe,ParamSet.MaxHight,0,255); |
CHK_POTI(Parameter_Luftdruck_D,ParamSet.AirPressure_D,0,100); |
CHK_POTI(Parameter_Hoehe_P,ParamSet.Hight_P,0,100); |
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,ParamSet.Hight_ACC_Effect,0,255); |
CHK_POTI(Parameter_CompassYawEffect,ParamSet.CompassYawEffect,0,255); |
CHK_POTI(Parameter_Gyro_P,ParamSet.Gyro_P,10,255); |
CHK_POTI(Parameter_Gyro_I,ParamSet.Gyro_I,0,255); |
CHK_POTI(Parameter_I_Factor,ParamSet.I_Factor,0,255); |
CHK_POTI(Parameter_UserParam1,ParamSet.UserParam1,0,255); |
CHK_POTI(Parameter_UserParam2,ParamSet.UserParam2,0,255); |
CHK_POTI(Parameter_UserParam3,ParamSet.UserParam3,0,255); |
CHK_POTI(Parameter_UserParam4,ParamSet.UserParam4,0,255); |
CHK_POTI(Parameter_UserParam5,ParamSet.UserParam5,0,255); |
CHK_POTI(Parameter_UserParam6,ParamSet.UserParam6,0,255); |
CHK_POTI(Parameter_UserParam7,ParamSet.UserParam7,0,255); |
CHK_POTI(Parameter_UserParam8,ParamSet.UserParam8,0,255); |
CHK_POTI(Parameter_ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
CHK_POTI(Parameter_LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
CHK_POTI(Parameter_AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
CHK_POTI(Parameter_AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
CHK_POTI(Parameter_DynamicStability,ParamSet.DynamicStability,0,255); |
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.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
CHK_POTI(FCParam.AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
CHK_POTI(FCParam.AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
|
Ki = (float) Parameter_I_Factor * 0.0001; |
MAX_GAS = ParamSet.Gas_Max; |
MIN_GAS = ParamSet.Gas_Min; |
Ki = (float) FCParam.I_Factor * FACTOR_I; |
} |
|
|
483,8 → 502,8 |
{ |
SumPitch = 0; |
SumRoll = 0; |
Reading_Integral_Yaw = 0; |
Reading_Integral_Yaw2 = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
{ |
515,7 → 534,7 |
} |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Piep(GetActiveParamSet()); |
Beep(GetActiveParamSet()); |
} |
} |
else |
524,18 → 543,16 |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetParamByte(PID_ACC_PITCH,0xFF); // Werte löschen |
SetParamWord(PID_ACC_PITCH,0xFFFF); // Werte löschen |
MotorsOn = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
SetNeutral(); |
SetParamByte(PID_ACC_PITCH, NeutralAccX / 256); // ACC-NeutralWerte speichern |
SetParamByte(PID_ACC_PITCH+1,NeutralAccX % 256); // ACC-NeutralWerte speichern |
SetParamByte(PID_ACC_ROLL,NeutralAccY / 256); |
SetParamByte(PID_ACC_ROLL+1,NeutralAccY % 256); |
SetParamByte(PID_ACC_Z,(int)NeutralAccZ / 256); |
SetParamByte(PID_ACC_Z+1,(int)NeutralAccZ % 256); |
Piep(GetActiveParamSet()); |
// ACC-NeutralWerte speichern |
SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
Beep(GetActiveParamSet()); |
} |
} |
else delay_neutral = 0; |
557,8 → 574,8 |
modell_fliegt = 1; |
MotorsOn = 1; |
SetPointYaw = 0; |
Reading_Integral_Yaw = 0; |
Reading_Integral_Yaw2 = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
Reading_IntegralPitch = 0; |
Reading_IntegralRoll = 0; |
Reading_IntegralPitch2 = IntegralPitch; |
591,7 → 608,7 |
if(!NewPpmData-- || EmergencyLanding) |
{ |
int tmp_int; |
ParameterZuordnung(); |
ParameterMapping(); |
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
606,13 → 623,13 |
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
if(EmergencyLanding) {MaxStickPitch = 0; MaxStickRoll = 0;} |
|
GyroFactor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
IntegralFactor = ((float) Parameter_Gyro_I) / 44000; |
GyroFactor = ((float)FCParam.Gyro_P + 10.0) / 256.0; |
IntegralFactor = ((float) FCParam.Gyro_I) / 44000; |
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Digitale Steuerung per DubWise |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) |
#define KEY_VALUE (FCParam.UserParam1 * 4) //(Poti3 * 8) |
if(DubWiseKeys[1]) BeepTime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
632,7 → 649,7 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
{ |
StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P; |
StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
924,9 → 941,9 |
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_Integral_Yaw -= tmp_int; |
if(Reading_Integral_Yaw > 50000) Reading_Integral_Yaw = 50000; // begrenzen |
if(Reading_Integral_Yaw <-50000) Reading_Integral_Yaw =-50000; |
Reading_IntegralYaw -= tmp_int; |
if(Reading_IntegralYaw > 50000) Reading_IntegralYaw = 50000; // begrenzen |
if(Reading_IntegralYaw <-50000) Reading_IntegralYaw =-50000; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
951,11 → 968,11 |
CompassCourse = CompassHeading; |
StoreNewCompassCourse = 0; |
} |
w = (w * Parameter_CompassYawEffect) / 64; // auf die Wirkung normieren |
w = Parameter_CompassYawEffect - w; // Wirkung ggf drosseln |
w = (w * FCParam.CompassYawEffect) / 64; // auf die Wirkung normieren |
w = FCParam.CompassYawEffect - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
Reading_Integral_Yaw -= (CompassOffCourse * w) / 32; // nach Kompass ausrichten |
Reading_IntegralYaw -= (CompassOffCourse * w) / 32; // nach Kompass ausrichten |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
992,13 → 1009,13 |
*/ |
// DebugOut.Analog[9] = ReadingPitch; |
// DebugOut.Analog[9] = SetPointHight; |
// DebugOut.Analog[10] = Reading_Integral_Yaw / 128; |
// DebugOut.Analog[10] = Reading_IntegralYaw / 128; |
// DebugOut.Analog[11] = CompassCourse; |
// DebugOut.Analog[10] = Parameter_Gyro_I; |
// DebugOut.Analog[10] = FCParam.Gyro_I; |
// DebugOut.Analog[10] = ParamSet.Gyro_I; |
// DebugOut.Analog[9] = CompassOffCourse; |
// DebugOut.Analog[10] = GasMixingFraction; |
// DebugOut.Analog[3] = HoeheD * 32; |
// DebugOut.Analog[3] = HightD * 32; |
// DebugOut.Analog[4] = hoehenregler; |
} |
|
1012,7 → 1029,7 |
else ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor; |
if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor; |
else ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor; |
ReadingYaw = ReadingYaw * (2 * GyroFactor) + Integral_Yaw * IntegralFactor / 2; |
ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2; |
|
DebugOut.Analog[25] = IntegralRoll * IntegralFactor; |
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFactor); |
1038,9 → 1055,9 |
int tmp_int; |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) // Regler wird über Schalter gesteuert |
{ |
if(Parameter_MaxHoehe < 50) |
if(FCParam.MaxHight < 50) |
{ |
SetPointHight = ReadingHight - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
SetPointHight = ReadingHight - 20; // FCParam.MaxHight ist der PPM-Wert des Schalters |
HightControlActive = 0; |
} |
else |
1048,7 → 1065,7 |
} |
else |
{ |
SetPointHight = ((int) ExternHightValue + (int) Parameter_MaxHoehe) * (int)ParamSet.Hight_Gain - 20; |
SetPointHight = ((int) ExternHightValue + (int) FCParam.MaxHight) * (int)ParamSet.Hight_Gain - 20; |
HightControlActive = 1; |
} |
|
1055,10 → 1072,10 |
if(EmergencyLanding) SetPointHight = 0; |
h = ReadingHight; |
if((h > SetPointHight) && HightControlActive) // zu hoch --> drosseln |
{ h = ((h - SetPointHight) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
{ h = ((h - SetPointHight) * (int) FCParam.Hight_P) / 16; // Differenz bestimmen --> P-Anteil |
h = GasMixingFraction - h; // vom Gas abziehen |
h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
tmp_int = ((Reading_Integral_Top / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
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; |
1072,7 → 1089,7 |
GasMixingFraction = hoehenregler; |
} |
} |
if(GasMixingFraction > MAX_GAS - 20) GasMixingFraction = MAX_GAS - 20; |
if(GasMixingFraction > ParamSet.Gas_Max - 20) GasMixingFraction = ParamSet.Gas_Max - 20; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1086,8 → 1103,8 |
|
if(YawMixingFraction > (GasMixingFraction / 2)) YawMixingFraction = GasMixingFraction / 2; |
if(YawMixingFraction < -(GasMixingFraction / 2)) YawMixingFraction = -(GasMixingFraction / 2); |
if(YawMixingFraction > ((MAX_GAS - GasMixingFraction))) YawMixingFraction = ((MAX_GAS - GasMixingFraction)); |
if(YawMixingFraction < -((MAX_GAS - GasMixingFraction))) YawMixingFraction = -((MAX_GAS - GasMixingFraction)); |
if(YawMixingFraction > ((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = ((ParamSet.Gas_Max - GasMixingFraction)); |
if(YawMixingFraction < -((ParamSet.Gas_Max - GasMixingFraction))) YawMixingFraction = -((ParamSet.Gas_Max - GasMixingFraction)); |
|
if(GasMixingFraction < 20) YawMixingFraction = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1100,20 → 1117,20 |
if(SumPitch < -16000) SumPitch = -16000; |
pd_ergebnis = DiffPitch + Ki * SumPitch; // PI-Regler für Pitch |
// Motor Vorn |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
|
MotorValue = GasMixingFraction + pd_ergebnis + YawMixingFraction; // Mischer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Front = MotorValue; |
// Motor Heck |
MotorValue = GasMixingFraction - pd_ergebnis + YawMixingFraction; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Rear = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
1124,7 → 1141,7 |
if(SumRoll > 16000) SumRoll = 16000; |
if(SumRoll < -16000) SumRoll = -16000; |
pd_ergebnis = DiffRoll + Ki * SumRoll; // PI-Regler für Roll |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
tmp_int = (long)((long)FCParam.DynamicStability * (long)(GasMixingFraction + abs(YawMixingFraction)/2)) / 64; |
if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
// Motor Links |
1132,15 → 1149,15 |
#define GRENZE Poti1 |
|
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Left = MotorValue; |
// Motor Rechts |
MotorValue = GasMixingFraction - pd_ergebnis - YawMixingFraction; |
|
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > MAX_GAS) MotorValue = MAX_GAS; |
if (MotorValue < MIN_GAS) MotorValue = MIN_GAS; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Right = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |