Rev 754 | Rev 762 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 754 | Rev 761 | ||
---|---|---|---|
Line 121... | Line 121... | ||
121 | 121 | ||
122 | // stick values derived by rc channels readings |
122 | // stick values derived by rc channels readings |
123 | int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
123 | int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
124 | int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0; |
124 | int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0; |
125 | // stick values derived by uart inputs |
125 | // stick values derived by uart inputs |
Line 126... | Line 126... | ||
126 | int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
126 | int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
127 | 127 | ||
Line 128... | Line 128... | ||
128 | 128 | ||
Line 129... | Line 129... | ||
129 | 129 | ||
Line 171... | Line 171... | ||
171 | FCParam.Yaw_PosFeedback = 0; |
171 | FCParam.Yaw_PosFeedback = 0; |
172 | FCParam.Yaw_NegFeedback = 0; |
172 | FCParam.Yaw_NegFeedback = 0; |
173 | CalibMean(); |
173 | CalibMean(); |
174 | Delay_ms_Mess(100); |
174 | Delay_ms_Mess(100); |
175 | CalibMean(); |
175 | CalibMean(); |
176 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
176 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated? |
177 | { |
177 | { |
178 | if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
178 | if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
179 | } |
179 | } |
180 | AdNeutralPitch = AdValueGyrPitch; |
180 | AdNeutralPitch = AdValueGyrPitch; |
181 | AdNeutralRoll = AdValueGyrRoll; |
181 | AdNeutralRoll = AdValueGyrRoll; |
Line 201... | Line 201... | ||
201 | Reading_IntegralGyroYaw = 0; |
201 | Reading_IntegralGyroYaw = 0; |
202 | Reading_GyroPitch = 0; |
202 | Reading_GyroPitch = 0; |
203 | Reading_GyroRoll = 0; |
203 | Reading_GyroRoll = 0; |
204 | Reading_GyroYaw = 0; |
204 | Reading_GyroYaw = 0; |
205 | StartAirPressure = AirPressure; |
205 | StartAirPressure = AirPressure; |
206 | HightD = 0; |
206 | HeightD = 0; |
207 | Reading_Integral_Top = 0; |
207 | Reading_Integral_Top = 0; |
208 | CompassCourse = CompassHeading; |
208 | CompassCourse = CompassHeading; |
209 | BeepTime = 50; |
209 | BeepTime = 50; |
210 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
210 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
211 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
211 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
212 | ExternHightValue = 0; |
212 | ExternHeightValue = 0; |
213 | GPS_Neutral(); |
213 | GPS_Neutral(); |
214 | } |
214 | } |
Line 215... | Line 215... | ||
215 | 215 | ||
216 | /************************************************************************/ |
216 | /************************************************************************/ |
Line 407... | Line 407... | ||
407 | /************************************************************************/ |
407 | /************************************************************************/ |
408 | void ParameterMapping(void) |
408 | void ParameterMapping(void) |
409 | { |
409 | { |
Line 410... | Line 410... | ||
410 | 410 | ||
411 | #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;} |
411 | #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;} |
412 | CHK_POTI(FCParam.MaxHight,ParamSet.MaxHight,0,255); |
412 | CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255); |
413 | CHK_POTI(FCParam.Hight_D,ParamSet.Hight_D,0,100); |
413 | CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100); |
414 | CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100); |
414 | CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100); |
415 | CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255); |
415 | CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255); |
416 | CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255); |
416 | CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255); |
417 | CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255); |
417 | CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255); |
418 | CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255); |
418 | CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255); |
419 | CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255); |
419 | CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255); |
Line 447... | Line 447... | ||
447 | static int32_t IntegralErrorPitch = 0; |
447 | static int32_t IntegralErrorPitch = 0; |
448 | static int32_t IntegralErrorRoll = 0; |
448 | static int32_t IntegralErrorRoll = 0; |
449 | static uint16_t RcLostTimer; |
449 | static uint16_t RcLostTimer; |
450 | static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
450 | static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
451 | static uint16_t Modell_Is_Flying = 0; |
451 | static uint16_t Modell_Is_Flying = 0; |
452 | static uint8_t HightControlActive = 0; |
452 | static uint8_t HeightControlActive = 0; |
453 | static int16_t HightControlThrust = 0; |
453 | static int16_t HeightControlThrust = 0; |
454 | static int8_t TimerDebugOut = 0; |
454 | static int8_t TimerDebugOut = 0; |
455 | static int8_t StoreNewCompassCourse = 0; |
455 | static int8_t StoreNewCompassCourse = 0; |
456 | static int32_t CorrectionPitch, CorrectionRoll; |
456 | static int32_t CorrectionPitch, CorrectionRoll; |
Line 457... | Line 457... | ||
457 | 457 | ||
Line 679... | Line 679... | ||
679 | else tmp_int = 0; |
679 | else tmp_int = 0; |
680 | ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
680 | ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
Line 681... | Line 681... | ||
681 | 681 | ||
682 | if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
682 | if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
683 | if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
683 | if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
684 | if(DubWiseKeys[0] & 2) ExternHightValue++; |
684 | if(DubWiseKeys[0] & 2) ExternHeightValue++; |
Line 685... | Line 685... | ||
685 | if(DubWiseKeys[0] & 16) ExternHightValue--; |
685 | if(DubWiseKeys[0] & 16) ExternHeightValue--; |
686 | 686 | ||
687 | StickPitch += ExternStickPitch / 8; |
687 | StickPitch += ExternStickPitch / 8; |
Line 695... | Line 695... | ||
695 | if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
695 | if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
696 | { |
696 | { |
697 | StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
697 | StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
698 | StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
698 | StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
699 | StickYaw += ExternControl.Yaw; |
699 | StickYaw += ExternControl.Yaw; |
700 | ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
700 | ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain; |
701 | if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust; |
701 | if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust; |
702 | } |
702 | } |
703 | // disable I part of gyro control feedback |
703 | // disable I part of gyro control feedback |
704 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
704 | if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
705 | // avoid negative scaling factors |
705 | // avoid negative scaling factors |
Line 1076... | Line 1076... | ||
1076 | DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
1076 | DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor; |
1077 | DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
1077 | DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor; |
1078 | DebugOut.Analog[2] = Mean_AccPitch; |
1078 | DebugOut.Analog[2] = Mean_AccPitch; |
1079 | DebugOut.Analog[3] = Mean_AccRoll; |
1079 | DebugOut.Analog[3] = Mean_AccRoll; |
1080 | DebugOut.Analog[4] = Reading_GyroYaw; |
1080 | DebugOut.Analog[4] = Reading_GyroYaw; |
1081 | DebugOut.Analog[5] = ReadingHight; |
1081 | DebugOut.Analog[5] = ReadingHeight; |
1082 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
1082 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
1083 | DebugOut.Analog[8] = CompassHeading; |
1083 | DebugOut.Analog[8] = CompassHeading; |
1084 | DebugOut.Analog[9] = UBat; |
1084 | DebugOut.Analog[9] = UBat; |
1085 | DebugOut.Analog[10] = SenderOkay; |
1085 | DebugOut.Analog[10] = SenderOkay; |
1086 | DebugOut.Analog[16] = Mean_AccTop; |
1086 | DebugOut.Analog[16] = Mean_AccTop; |
Line 1096... | Line 1096... | ||
1096 | DebugOut.Analog[23] = motor_rx[6]; |
1096 | DebugOut.Analog[23] = motor_rx[6]; |
1097 | DebugOut.Analog[24] = motor_rx[7]; |
1097 | DebugOut.Analog[24] = motor_rx[7]; |
1098 | DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
1098 | DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
Line 1099... | Line 1099... | ||
1099 | 1099 | ||
1100 | DebugOut.Analog[9] = Reading_GyroPitch; |
1100 | DebugOut.Analog[9] = Reading_GyroPitch; |
1101 | DebugOut.Analog[9] = SetPointHight; |
1101 | DebugOut.Analog[9] = SetPointHeight; |
1102 | DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128; |
1102 | DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128; |
1103 | DebugOut.Analog[11] = CompassCourse; |
1103 | DebugOut.Analog[11] = CompassCourse; |
1104 | DebugOut.Analog[10] = FCParam.Gyro_I; |
1104 | DebugOut.Analog[10] = FCParam.Gyro_I; |
1105 | DebugOut.Analog[10] = ParamSet.Gyro_I; |
1105 | DebugOut.Analog[10] = ParamSet.Gyro_I; |
1106 | DebugOut.Analog[9] = CompassOffCourse; |
1106 | DebugOut.Analog[9] = CompassOffCourse; |
1107 | DebugOut.Analog[10] = ThrustMixFraction; |
1107 | DebugOut.Analog[10] = ThrustMixFraction; |
1108 | DebugOut.Analog[3] = HightD * 32; |
1108 | DebugOut.Analog[3] = HeightD * 32; |
1109 | DebugOut.Analog[4] = HightControlThrust; |
1109 | DebugOut.Analog[4] = HeightControlThrust; |
1110 | */ |
1110 | */ |
Line 1111... | Line 1111... | ||
1111 | } |
1111 | } |
1112 | 1112 | ||
Line 1132... | Line 1132... | ||
1132 | if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR; |
1132 | if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR; |
1133 | if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
1133 | if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
1134 | if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
1134 | if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
Line 1135... | Line 1135... | ||
1135 | 1135 | ||
1136 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1136 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1137 | // Hight Control |
1137 | // Height Control |
1138 | // The higth control algorithm reduces the thrust but does not increase the thrust. |
1138 | // The higth control algorithm reduces the thrust but does not increase the thrust. |
1139 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1139 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1140 | // If hight control is activated and no emergency landing is active |
1140 | // If hight control is activated and no emergency landing is active |
1141 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
1141 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
1142 | { |
1142 | { |
1143 | int tmp_int; |
1143 | int tmp_int; |
1144 | // if hight control is activated by an rc channel |
1144 | // if hight control is activated by an rc channel |
1145 | if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
1145 | if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
1146 | { // check if parameter is less than activation threshold |
1146 | { // check if parameter is less than activation threshold |
1147 | if(FCParam.MaxHight < 50) |
1147 | if(FCParam.MaxHeight < 50) |
1148 | { |
1148 | { |
1149 | SetPointHight = ReadingHight - 20; // update SetPoint with current reading |
1149 | SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading |
1150 | HightControlActive = 0; // disable hight control |
1150 | HeightControlActive = 0; // disable hight control |
1151 | } |
1151 | } |
1152 | else HightControlActive = 1; // enable hight control |
1152 | else HeightControlActive = 1; // enable hight control |
1153 | } |
1153 | } |
1154 | else // no switchable hight control |
1154 | else // no switchable hight control |
1155 | { |
1155 | { |
1156 | SetPointHight = ((int16_t) ExternHightValue + (int16_t) FCParam.MaxHight) * (int16_t)ParamSet.Hight_Gain - 20; |
1156 | SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20; |
1157 | HightControlActive = 1; |
1157 | HeightControlActive = 1; |
1158 | } |
1158 | } |
1159 | // get current hight |
1159 | // get current hight |
1160 | h = ReadingHight; |
1160 | h = ReadingHeight; |
1161 | // if current hight is above the setpoint reduce thrust |
1161 | // if current hight is above the setpoint reduce thrust |
1162 | if((h > SetPointHight) && HightControlActive) |
1162 | if((h > SetPointHeight) && HeightControlActive) |
1163 | { |
1163 | { |
1164 | // hight difference -> P control part |
1164 | // hight difference -> P control part |
1165 | h = ((h - SetPointHight) * (int16_t) FCParam.Hight_P) / 16; |
1165 | h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16; |
1166 | h = ThrustMixFraction - h; // reduce gas |
1166 | h = ThrustMixFraction - h; // reduce gas |
1167 | // higth gradient --> D control part |
1167 | // higth gradient --> D control part |
1168 | h -= (HightD * FCParam.Hight_D) / 8; // D control part |
1168 | h -= (HeightD * FCParam.Height_D) / 8; // D control part |
1169 | // acceleration sensor effect |
1169 | // acceleration sensor effect |
1170 | tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Hight_ACC_Effect) / 32; |
1170 | tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32; |
1171 | if(tmp_int > 50) tmp_int = 50; |
1171 | if(tmp_int > 50) tmp_int = 50; |
1172 | if(tmp_int < -50) tmp_int = -50; |
1172 | if(tmp_int < -50) tmp_int = -50; |
1173 | h -= tmp_int; |
1173 | h -= tmp_int; |
1174 | // update hight control thrust |
1174 | // update hight control thrust |
1175 | HightControlThrust = (HightControlThrust*15 + h) / 16; |
1175 | HeightControlThrust = (HeightControlThrust*15 + h) / 16; |
1176 | // limit thrust reduction |
1176 | // limit thrust reduction |
1177 | if(HightControlThrust < ParamSet.Hight_MinThrust) |
1177 | if(HeightControlThrust < ParamSet.Height_MinThrust) |
1178 | { |
1178 | { |
1179 | if(ThrustMixFraction >= ParamSet.Hight_MinThrust) HightControlThrust = ParamSet.Hight_MinThrust; |
1179 | if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust; |
1180 | // allows landing also if thrust stick is reduced below min thrust on hight control |
1180 | // allows landing also if thrust stick is reduced below min thrust on hight control |
1181 | if(ThrustMixFraction < ParamSet.Hight_MinThrust) HightControlThrust = ThrustMixFraction; |
1181 | if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction; |
1182 | } |
1182 | } |
1183 | // limit thrust to stick setting |
1183 | // limit thrust to stick setting |
1184 | if(HightControlThrust > ThrustMixFraction) HightControlThrust = ThrustMixFraction; |
1184 | if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction; |
1185 | ThrustMixFraction = HightControlThrust; |
1185 | ThrustMixFraction = HeightControlThrust; |
1186 | } |
1186 | } |
1187 | } |
1187 | } |
1188 | // limit thrust to parameter setting |
1188 | // limit thrust to parameter setting |
1189 | if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20; |
1189 | if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20; |