Rev 1638 | Rev 1642 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1638 | Rev 1639 | ||
---|---|---|---|
Line 63... | Line 63... | ||
63 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
63 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
64 | int TrimNick, TrimRoll; |
64 | int TrimNick, TrimRoll; |
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
68 | volatile float NeutralAccZ = 0; |
68 | int NeutralAccZ = 0; |
69 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
69 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
70 | long IntegralNick = 0,IntegralNick2 = 0; |
70 | long IntegralNick = 0,IntegralNick2 = 0; |
71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
73 | long Integral_Gier = 0; |
73 | long Integral_Gier = 0; |
Line 170... | Line 170... | ||
170 | else motor = neu - (alt - neu)*1; |
170 | else motor = neu - (alt - neu)*1; |
171 | //if(Poti2 < 20) return(neu); |
171 | //if(Poti2 < 20) return(neu); |
172 | return(motor); |
172 | return(motor); |
173 | } |
173 | } |
Line -... | Line 174... | ||
- | 174 | ||
- | 175 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 176 | // Debugwerte zuordnen |
|
- | 177 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 178 | void CopyDebugValues(void) |
|
- | 179 | { |
|
- | 180 | ||
- | 181 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
|
- | 182 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
|
- | 183 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
|
- | 184 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
|
- | 185 | DebugOut.Analog[4] = MesswertGier; |
|
- | 186 | DebugOut.Analog[5] = HoehenWert/5; |
|
- | 187 | DebugOut.Analog[6] = AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
|
- | 188 | DebugOut.Analog[8] = KompassValue; |
|
- | 189 | DebugOut.Analog[9] = UBat; |
|
- | 190 | DebugOut.Analog[10] = SenderOkay; |
|
- | 191 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
|
- | 192 | DebugOut.Analog[16] = NeutralAccZ; |
|
- | 193 | DebugOut.Analog[18] = TransmitBlConfig; |
|
- | 194 | //DebugOut.Analog[16] = Motor[0].Temperature; |
|
- | 195 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
|
- | 196 | // DebugOut.Analog[18] = VarioMeter; |
|
- | 197 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
|
- | 198 | DebugOut.Analog[20] = ServoNickValue; |
|
- | 199 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
|
- | 200 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
|
- | 201 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
|
- | 202 | // DebugOut.Analog[24] = MesswertNick/2; |
|
- | 203 | // DebugOut.Analog[25] = MesswertRoll/2; |
|
- | 204 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
|
- | 205 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
|
- | 206 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
|
- | 207 | //DebugOut.Analog[28] = I2CError; |
|
- | 208 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
|
- | 209 | DebugOut.Analog[30] = GPS_Nick; |
|
- | 210 | DebugOut.Analog[31] = GPS_Roll; |
|
- | 211 | } |
|
- | 212 | ||
- | 213 | ||
174 | 214 | ||
175 | void Piep(unsigned char Anzahl, unsigned int dauer) |
215 | void Piep(unsigned char Anzahl, unsigned int dauer) |
176 | { |
216 | { |
177 | if(MotorenEin) return; //auf keinen Fall im Flug! |
217 | if(MotorenEin) return; //auf keinen Fall im Flug! |
178 | while(Anzahl--) |
218 | while(Anzahl--) |
Line 231... | Line 271... | ||
231 | Parameter_AchsKopplung1 = 0; |
271 | Parameter_AchsKopplung1 = 0; |
232 | Parameter_AchsKopplung2 = 0; |
272 | Parameter_AchsKopplung2 = 0; |
Line 233... | Line 273... | ||
233 | 273 | ||
Line 234... | Line 274... | ||
234 | ExpandBaro = 0; |
274 | ExpandBaro = 0; |
- | 275 | ||
Line -... | Line 276... | ||
- | 276 | TransmitBlConfig = 1; |
|
235 | 277 | motorread = 0; |
|
Line 236... | Line 278... | ||
236 | CalibrierMittelwert(); |
278 | |
Line 237... | Line 279... | ||
237 | 279 | CalibrierMittelwert(); |
|
Line 263... | Line 305... | ||
263 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
305 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
264 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
306 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
265 | NeutralAccZ = Aktuell_az; |
307 | NeutralAccZ = Aktuell_az; |
Line 266... | Line 308... | ||
266 | 308 | ||
267 | // Save ACC neutral settings to eeprom |
309 | // Save ACC neutral settings to eeprom |
268 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccY); |
310 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
269 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccX); |
311 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
270 | SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ); |
312 | SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ); |
271 | } |
313 | } |
272 | else |
314 | else |
273 | { |
315 | { |
Line 517... | Line 559... | ||
517 | DebugOut.Analog[15] = Motor[3].SetPoint; |
559 | DebugOut.Analog[15] = Motor[3].SetPoint; |
Line 518... | Line 560... | ||
518 | 560 | ||
519 | //Start I2C Interrupt Mode |
561 | //Start I2C Interrupt Mode |
520 | twi_state = 0; |
562 | twi_state = 0; |
521 | motor = 0; |
563 | motor = 0; |
522 | i2c_start(); |
564 | I2C_Start(); |
Line 523... | Line 565... | ||
523 | } |
565 | } |
Line 1121... | Line 1163... | ||
1121 | MittelIntegralNick2 = 0; |
1163 | MittelIntegralNick2 = 0; |
1122 | MittelIntegralRoll2 = 0; |
1164 | MittelIntegralRoll2 = 0; |
1123 | ZaehlMessungen = 0; |
1165 | ZaehlMessungen = 0; |
1124 | } // ZaehlMessungen >= ABGLEICH_ANZAHL |
1166 | } // ZaehlMessungen >= ABGLEICH_ANZAHL |
Line 1125... | Line -... | ||
1125 | - | ||
1126 | 1167 | ||
1127 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1168 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1128 | // Gieren |
1169 | // Gieren |
1129 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1170 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1130 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1171 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
Line 1191... | Line 1232... | ||
1191 | } |
1232 | } |
1192 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1233 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1193 | } |
1234 | } |
1194 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1235 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1195 | } |
1236 | } |
1196 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1197 | - | ||
1198 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1199 | // Debugwerte zuordnen |
- | |
1200 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1201 | if(!TimerWerteausgabe--) |
- | |
1202 | { |
- | |
1203 | TimerWerteausgabe = 24; |
- | |
1204 | - | ||
1205 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
- | |
1206 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
- | |
1207 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
- | |
1208 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
- | |
1209 | DebugOut.Analog[4] = MesswertGier; |
- | |
1210 | DebugOut.Analog[5] = HoehenWert/5; |
- | |
1211 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
- | |
1212 | DebugOut.Analog[8] = KompassValue; |
- | |
1213 | DebugOut.Analog[9] = UBat; |
- | |
1214 | DebugOut.Analog[10] = SenderOkay; |
- | |
1215 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
- | |
1216 | //DebugOut.Analog[16] = Motor[0].Temperature; |
- | |
1217 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
- | |
1218 | // DebugOut.Analog[18] = VarioMeter; |
- | |
1219 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
- | |
1220 | DebugOut.Analog[20] = ServoNickValue; |
- | |
1221 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
- | |
1222 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
- | |
1223 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
- | |
1224 | // DebugOut.Analog[24] = MesswertNick/2; |
- | |
1225 | // DebugOut.Analog[25] = MesswertRoll/2; |
- | |
1226 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
- | |
1227 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
- | |
1228 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
- | |
1229 | //DebugOut.Analog[28] = I2CError; |
- | |
1230 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
- | |
1231 | DebugOut.Analog[30] = GPS_Nick; |
- | |
1232 | DebugOut.Analog[31] = GPS_Roll; |
- | |
1233 | } |
- | |
Line 1234... | Line 1237... | ||
1234 | 1237 | ||
1235 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1238 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1236 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1239 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1237 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1240 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1262... | Line 1265... | ||
1262 | // Höhenregelung |
1265 | // Höhenregelung |
1263 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1266 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1264 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1267 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1265 | if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1268 | if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1266 | GasMischanteil *= STICK_GAIN; |
1269 | GasMischanteil *= STICK_GAIN; |
1267 | - | ||
1268 | // if height control is activated |
1270 | // if height control is activated |
1269 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1271 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1270 | { |
1272 | { |
1271 | #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1273 | #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1272 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
1274 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |