Rev 2318 | Rev 2320 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2318 | Rev 2319 | ||
|---|---|---|---|
| Line 269... | Line 269... | ||
| 269 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
269 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
| 270 | } |
270 | } |
| Line 271... | Line 271... | ||
| 271 | 271 | ||
| 272 | //############################################################################ |
272 | //############################################################################ |
| - | 273 | // Nullwerte ermitteln |
|
| - | 274 | // Parameter: 0 -> after switch on (ignore ACC-Z fault) |
|
| - | 275 | // Parameter: 1 -> before Start |
|
| 273 | // Nullwerte ermitteln |
276 | // Parameter: 2 -> calibrate and store ACC |
| 274 | unsigned char SetNeutral(unsigned char AccAdjustment) // retuns: "sucess" |
277 | unsigned char SetNeutral(unsigned char AdjustmentMode) // retuns: "sucess" |
| 275 | //############################################################################ |
278 | //############################################################################ |
| 276 | { |
279 | { |
| 277 | unsigned char i, sucess = 1; |
280 | unsigned char i, sucess = 1; |
| 278 | unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0; |
281 | unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0; |
| Line 316... | Line 319... | ||
| 316 | NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
319 | NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| Line 317... | Line 320... | ||
| 317 | 320 | ||
| 318 | StartNeutralRoll = AdNeutralRoll; |
321 | StartNeutralRoll = AdNeutralRoll; |
| Line 319... | Line 322... | ||
| 319 | StartNeutralNick = AdNeutralNick; |
322 | StartNeutralNick = AdNeutralNick; |
| 320 | 323 | ||
| 321 | if(AccAdjustment) |
324 | if(AdjustmentMode == 2) |
| 322 | { |
325 | { |
| 323 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
326 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
| 324 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
327 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
| Line 390... | Line 393... | ||
| 390 | tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
393 | tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| 391 | tilt1 = (int16_t)ihypot(tilt1,tilt2); // tilt angle over all |
394 | tilt1 = (int16_t)ihypot(tilt1,tilt2); // tilt angle over all |
| 392 | CosAttitude = c_cos_8192(tilt1); |
395 | CosAttitude = c_cos_8192(tilt1); |
| 393 | NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512; |
396 | NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512; |
| 394 | if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle |
397 | if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle |
| 395 | if(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
398 | if(AdjustmentMode != 0 && ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
| 396 | - | ||
| 397 | #else |
399 | #else |
| 398 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
400 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
| 399 | #endif |
401 | #endif |
| Line 400... | Line 402... | ||
| 400 | 402 | ||
| Line 850... | Line 852... | ||
| 850 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
852 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| 851 | { |
853 | { |
| 852 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
854 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| 853 | } |
855 | } |
| 854 | // ServoActive = 0; |
856 | // ServoActive = 0; |
| 855 | CalibrationDone = SetNeutral(0); |
857 | CalibrationDone = SetNeutral(1); |
| 856 | ServoActive = 1; |
858 | ServoActive = 1; |
| 857 | DDRD |=0x80; // enable J7 -> Servo signal |
859 | DDRD |=0x80; // enable J7 -> Servo signal |
| 858 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
860 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| 859 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
861 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
| 860 | else |
862 | else |
| Line 871... | Line 873... | ||
| 871 | if(++delay_neutral > 200) // nicht sofort |
873 | if(++delay_neutral > 200) // nicht sofort |
| 872 | { |
874 | { |
| 873 | MotorenEin = 0; |
875 | MotorenEin = 0; |
| 874 | delay_neutral = 0; |
876 | delay_neutral = 0; |
| 875 | modell_fliegt = 0; |
877 | modell_fliegt = 0; |
| 876 | CalibrationDone = SetNeutral(1); |
878 | CalibrationDone = SetNeutral(2); // store ACC values into EEPROM |
| 877 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
879 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| 878 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
880 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
| 879 | else |
881 | else |
| 880 | if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
882 | if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
| 881 | else SpeakHoTT = SPEAK_CALIBRATE; |
883 | else SpeakHoTT = SPEAK_CALIBRATE; |