Rev 2669 | Rev 2676 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2669 | Rev 2672 | ||
|---|---|---|---|
| Line 62... | Line 62... | ||
| 62 | unsigned int BaroExpandActive = 0; |
62 | unsigned int BaroExpandActive = 0; |
| 63 | int MesswertNick,MesswertRoll,MesswertGier,RohMesswertNick,RohMesswertRoll; |
63 | int MesswertNick,MesswertRoll,MesswertGier,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 BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0; |
66 | int BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0; |
| - | 67 | int LastFlightNeutralNick = 0,LastFlightNeutralRoll = 0,LastFlightNeutralGier = 0; |
|
| 67 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
68 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
| 68 | unsigned int NeutralAccX=0, NeutralAccY=0; |
69 | unsigned int NeutralAccX=0, NeutralAccY=0; |
| 69 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
70 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
| 70 | int NeutralAccZ = 0; |
71 | int NeutralAccZ = 0; |
| 71 | signed char NeutralAccZfine = 0; |
72 | signed char NeutralAccZfine = 0; |
| Line 200... | Line 201... | ||
| 200 | #define OPA_OFFSET_STEP 5 |
201 | #define OPA_OFFSET_STEP 5 |
| 201 | #else |
202 | #else |
| 202 | #define OPA_OFFSET_STEP 10 |
203 | #define OPA_OFFSET_STEP 10 |
| 203 | #endif |
204 | #endif |
| Line -... | Line 205... | ||
| - | 205 | ||
| - | 206 | #define MAX_DRIFT_NR 32 |
|
| - | 207 | #define MAX_DRIFT_YAW 8 |
|
| 204 | 208 | ||
| 205 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
209 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 206 | // Debugwerte zuordnen |
210 | // Debugwerte zuordnen |
| 207 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
211 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 208 | void CopyDebugValues(void) |
212 | void CopyDebugValues(void) |
| Line 298... | Line 302... | ||
| 298 | BoatNeutralNick = AdNeutralNick; |
302 | BoatNeutralNick = AdNeutralNick; |
| 299 | BoatNeutralRoll = AdNeutralRoll; |
303 | BoatNeutralRoll = AdNeutralRoll; |
| 300 | BoatNeutralGier = AdNeutralGier; |
304 | BoatNeutralGier = AdNeutralGier; |
| 301 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
305 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
| 302 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
306 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
| - | 307 | ||
| 303 | SetParamWord(PID_GYRO_NICK,(uint16_t)BoatNeutralNick); |
308 | SetParamWord(PID_GYRO_NICK,(uint16_t)BoatNeutralNick); |
| 304 | SetParamWord(PID_GYRO_ROLL,(uint16_t)BoatNeutralRoll); |
309 | SetParamWord(PID_GYRO_ROLL,(uint16_t)BoatNeutralRoll); |
| 305 | SetParamWord(PID_GYRO_YAW,(uint16_t)BoatNeutralGier); |
310 | SetParamWord(PID_GYRO_YAW,(uint16_t)BoatNeutralGier); |
| - | 311 | ||
| - | 312 | SetParamWord(EE_LAST_GYRO_NICK,(uint16_t)BoatNeutralNick); |
|
| - | 313 | SetParamWord(EE_LAST_GYRO_ROLL,(uint16_t)BoatNeutralRoll); |
|
| - | 314 | SetParamWord(EE_LAST_GYRO_YAW,(uint16_t)BoatNeutralGier); |
|
| - | 315 | } |
|
| - | 316 | ||
| - | 317 | void StoreLastDriftcompensation(void) |
|
| - | 318 | { |
|
| - | 319 | int last_nick,last_roll,last_yaw; |
|
| - | 320 | ||
| - | 321 | last_nick = AdNeutralNick; |
|
| - | 322 | last_roll = AdNeutralRoll; |
|
| - | 323 | last_yaw = AdNeutralGier; |
|
| - | 324 | ||
| - | 325 | if(last_nick > BoatNeutralNick + MAX_DRIFT_NR) last_yaw = BoatNeutralNick + MAX_DRIFT_NR; |
|
| - | 326 | if(last_nick < BoatNeutralNick - MAX_DRIFT_NR) last_yaw = BoatNeutralNick - MAX_DRIFT_NR; |
|
| - | 327 | if(last_roll > BoatNeutralRoll + MAX_DRIFT_NR) last_roll = BoatNeutralRoll + MAX_DRIFT_NR; |
|
| - | 328 | if(last_roll < BoatNeutralRoll - MAX_DRIFT_NR) last_roll = BoatNeutralRoll - MAX_DRIFT_NR; |
|
| - | 329 | if(last_yaw > BoatNeutralGier + MAX_DRIFT_YAW) last_yaw = BoatNeutralGier + MAX_DRIFT_YAW; |
|
| - | 330 | if(last_yaw < BoatNeutralGier - MAX_DRIFT_YAW) last_yaw = BoatNeutralGier - MAX_DRIFT_YAW; |
|
| - | 331 | ||
| - | 332 | SetParamWord(EE_LAST_GYRO_NICK,(uint16_t)last_nick); |
|
| - | 333 | SetParamWord(EE_LAST_GYRO_ROLL,(uint16_t)last_roll); |
|
| - | 334 | SetParamWord(EE_LAST_GYRO_YAW,(uint16_t)last_yaw); |
|
| 306 | } |
335 | } |
| Line -... | Line 336... | ||
| - | 336 | ||
| 307 | 337 | ||
| 308 | //############################################################################ |
338 | //############################################################################ |
| 309 | // Nullwerte ermitteln |
339 | // Nullwerte ermitteln |
| 310 | // Parameter: 0 -> after switch on (ignore ACC-Z fault) |
340 | // Parameter: 0 -> after switch on (ignore ACC-Z fault) |
| 311 | // Parameter: 1 -> before Start |
341 | // Parameter: 1 -> before Start |
| Line 360... | Line 390... | ||
| 360 | OCR0B = 255 - OCR0A; |
390 | OCR0B = 255 - OCR0A; |
| 361 | AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
391 | AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| 362 | AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
392 | AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
| 363 | AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
393 | AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| 364 | NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
394 | NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
| - | 395 | //AdNeutralGier -= 20; |
|
| Line 365... | Line 396... | ||
| 365 | 396 | ||
| 366 | StartNeutralRoll = AdNeutralRoll; |
397 | StartNeutralRoll = AdNeutralRoll; |
| 367 | StartNeutralNick = AdNeutralNick; |
398 | StartNeutralNick = AdNeutralNick; |
| 368 | VersionInfo.HardwareError[1] &= ~FC_ERROR1_ACC_NOT_CAL; |
399 | VersionInfo.HardwareError[1] &= ~FC_ERROR1_ACC_NOT_CAL; |
| Line 379... | Line 410... | ||
| 379 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
410 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
| 380 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
411 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
| 381 | // strange settings? |
412 | // strange settings? |
| 382 | if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
413 | if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
| 383 | { |
414 | { |
| 384 | printf("\n\rACC not calibrated!\r\n"); |
415 | printf("\r\nACC not calibrated!\r\n"); |
| 385 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
416 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
| 386 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
417 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
| 387 | VersionInfo.HardwareError[1] |= FC_ERROR1_ACC_NOT_CAL; |
418 | VersionInfo.HardwareError[1] |= FC_ERROR1_ACC_NOT_CAL; |
| 388 | sucess = 0; |
419 | sucess = 0; |
| 389 | } |
420 | } |
| Line 390... | Line 421... | ||
| 390 | 421 | ||
| 391 | // restore from eeprom |
422 | // restore from eeprom |
| 392 | BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK); |
423 | BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK); |
| 393 | BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL); |
424 | BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL); |
| - | 425 | BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW); |
|
| - | 426 | LastFlightNeutralNick = (int16_t)GetParamWord(EE_LAST_GYRO_NICK); |
|
| - | 427 | LastFlightNeutralRoll = (int16_t)GetParamWord(EE_LAST_GYRO_ROLL); |
|
| - | 428 | LastFlightNeutralGier = (int16_t)GetParamWord(EE_LAST_GYRO_YAW); |
|
| - | 429 | //DebugOut.Analog[16] = LastFlightNeutralNick; |
|
| - | 430 | //DebugOut.Analog[17] = LastFlightNeutralRoll; |
|
| - | 431 | //DebugOut.Analog[18] = LastFlightNeutralGier; |
|
| - | 432 | ||
| - | 433 | if(abs(BoatNeutralNick - LastFlightNeutralNick) > 200) LastFlightNeutralNick = BoatNeutralNick; |
|
| - | 434 | if(abs(BoatNeutralRoll - LastFlightNeutralRoll) > 200) LastFlightNeutralRoll = BoatNeutralRoll; |
|
| Line 394... | Line 435... | ||
| 394 | BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW); |
435 | if(abs(BoatNeutralGier - LastFlightNeutralGier) > 50) LastFlightNeutralGier = BoatNeutralGier; |
| 395 | 436 | ||
| 396 | if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom |
437 | if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom |
| 397 | { |
438 | { |
| 398 | // strange settings? |
439 | // strange settings? |
| 399 | if(((unsigned int) BoatNeutralNick > (600 * 16)) || ((unsigned int) BoatNeutralNick < (400 * 16)) |
440 | if(((unsigned int) BoatNeutralNick > (600 * 16)) || ((unsigned int) BoatNeutralNick < (400 * 16)) |
| 400 | || ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16)) |
441 | || ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16)) |
| 401 | || ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2))) |
442 | || ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2))) |
| 402 | { |
443 | { |
| 403 | printf("\n\rGyro calibration data not valid\r\n"); |
444 | printf("\r\nGyro calibration data not valid\r\n"); |
| 404 | sucess = 0; |
445 | sucess = 0; |
| 405 | FC_StatusFlags3 &= ~FC_STATUS3_BOAT; |
446 | FC_StatusFlags3 &= ~FC_STATUS3_BOAT; |
| 406 | } |
447 | } |
| Line 410... | Line 451... | ||
| 410 | AdNeutralRoll = BoatNeutralRoll; |
451 | AdNeutralRoll = BoatNeutralRoll; |
| 411 | AdNeutralGier = BoatNeutralGier; |
452 | AdNeutralGier = BoatNeutralGier; |
| 412 | } |
453 | } |
| 413 | } |
454 | } |
| 414 | } |
455 | } |
| - | 456 | ||
| - | 457 | // ist it within the Boat-Values? |
|
| - | 458 | if(abs((AdNeutralGier - BoatNeutralGier) > MAX_DRIFT_YAW) || abs((AdNeutralNick - BoatNeutralNick) > MAX_DRIFT_NR) || abs((AdNeutralRoll - BoatNeutralRoll) > MAX_DRIFT_NR)) |
|
| - | 459 | { |
|
| - | 460 | sucess = 0; |
|
| - | 461 | if(AdjustmentMode == 1) SpeakHoTT = SPEAK_ERR_CALIBARTION; // calibration before start |
|
| - | 462 | AdNeutralNick = BoatNeutralNick; |
|
| - | 463 | AdNeutralRoll = BoatNeutralRoll; |
|
| - | 464 | AdNeutralGier = BoatNeutralGier; |
|
| - | 465 | } |
|
| - | 466 | ||
| - | 467 | // make average from these three values |
|
| - | 468 | AdNeutralGier = (BoatNeutralGier + LastFlightNeutralGier + AdNeutralGier) / 3; |
|
| - | 469 | AdNeutralNick = (BoatNeutralNick + LastFlightNeutralNick + AdNeutralNick) / 3; |
|
| - | 470 | AdNeutralRoll = (BoatNeutralRoll + LastFlightNeutralRoll + AdNeutralRoll) / 3; |
|
| - | 471 | ||
| 415 | EEAR = EE_DUMMY; // Set the EEPROM Address pointer to an unused space |
472 | EEAR = EE_DUMMY; // Set the EEPROM Address pointer to an unused space |
| 416 | MesswertNick = 0; |
473 | MesswertNick = 0; |
| 417 | MesswertRoll = 0; |
474 | MesswertRoll = 0; |
| 418 | MesswertGier = 0; |
475 | MesswertGier = 0; |
| 419 | Delay_ms_Mess(200); |
476 | Delay_ms_Mess(200); |
| Line 1233... | Line 1290... | ||
| 1233 | } |
1290 | } |
| 1234 | if(++delay_ausschalten > 250 || Partner_StatusFlags3 & FC_STATUS3_MOTORS_STOPPED_BY_RC) // nicht sofort oder wenn der Partner schon aus ist |
1291 | if(++delay_ausschalten > 250 || Partner_StatusFlags3 & FC_STATUS3_MOTORS_STOPPED_BY_RC) // nicht sofort oder wenn der Partner schon aus ist |
| 1235 | { |
1292 | { |
| 1236 | FC_StatusFlags3 |= FC_STATUS3_MOTORS_STOPPED_BY_RC; // that informs the slave to disarm the Motors |
1293 | FC_StatusFlags3 |= FC_STATUS3_MOTORS_STOPPED_BY_RC; // that informs the slave to disarm the Motors |
| 1237 | Delete_Stoppflag_Timer = 2; // 1-2 seconds |
1294 | Delete_Stoppflag_Timer = 2; // 1-2 seconds |
| - | 1295 | if(!NC_ErrorCode && modell_fliegt > 60 * 500 && MotorenEin) StoreLastDriftcompensation(); |
|
| 1238 | MotorenEin = 0; |
1296 | MotorenEin = 0; |
| 1239 | delay_ausschalten = 0; |
1297 | delay_ausschalten = 0; |
| 1240 | modell_fliegt = 0; |
1298 | modell_fliegt = 0; |
| 1241 | FC_StatusFlags2 &= ~(FC_STATUS2_WAIT_FOR_TAKEOFF | FC_STATUS2_AUTO_STARTING | FC_STATUS2_AUTO_LANDING); |
1299 | FC_StatusFlags2 &= ~(FC_STATUS2_WAIT_FOR_TAKEOFF | FC_STATUS2_AUTO_STARTING | FC_STATUS2_AUTO_LANDING); |
| 1242 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1300 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| Line 1516... | Line 1574... | ||
| 1516 | if(DriftNick <-3000) { DriftNick = 0; AdNeutralNick--;} |
1574 | if(DriftNick <-3000) { DriftNick = 0; AdNeutralNick--;} |
| 1517 | if(DriftRoll > 3000) { DriftRoll = 0; AdNeutralRoll++;} |
1575 | if(DriftRoll > 3000) { DriftRoll = 0; AdNeutralRoll++;} |
| 1518 | if(DriftRoll <-3000) { DriftRoll = 0; AdNeutralRoll--;} |
1576 | if(DriftRoll <-3000) { DriftRoll = 0; AdNeutralRoll--;} |
| 1519 | if(GierGyroFehler > 3500) { GierGyroFehler = 0; AdNeutralGier++; } |
1577 | if(GierGyroFehler > 3500) { GierGyroFehler = 0; AdNeutralGier++; } |
| 1520 | if(GierGyroFehler <-3500) { GierGyroFehler = 0; AdNeutralGier--; } |
1578 | if(GierGyroFehler <-3500) { GierGyroFehler = 0; AdNeutralGier--; } |
| - | 1579 | ||
| - | 1580 | if(AdNeutralNick > BoatNeutralNick + MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralNick + MAX_DRIFT_NR; |
|
| - | 1581 | if(AdNeutralNick < BoatNeutralNick - MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralNick - MAX_DRIFT_NR; |
|
| - | 1582 | if(AdNeutralRoll > BoatNeutralRoll + MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralRoll + MAX_DRIFT_NR; |
|
| - | 1583 | if(AdNeutralRoll < BoatNeutralRoll - MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralRoll - MAX_DRIFT_NR; |
|
| - | 1584 | if(AdNeutralGier > BoatNeutralGier + MAX_DRIFT_YAW) AdNeutralGier = BoatNeutralGier + MAX_DRIFT_YAW; |
|
| - | 1585 | if(AdNeutralGier < BoatNeutralGier - MAX_DRIFT_YAW) AdNeutralGier = BoatNeutralGier - MAX_DRIFT_YAW; |
|
| 1521 | } |
1586 | } |
| 1522 | else |
1587 | else |
| 1523 | { |
1588 | { |
| 1524 | DriftNick = 0; |
1589 | DriftNick = 0; |
| 1525 | DriftRoll = 0; |
1590 | DriftRoll = 0; |