64,6 → 64,7 |
int TrimNick, TrimRoll; |
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
int BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0; |
int LastFlightNeutralNick = 0,LastFlightNeutralRoll = 0,LastFlightNeutralGier = 0; |
int Mittelwert_AccNick, Mittelwert_AccRoll; |
unsigned int NeutralAccX=0, NeutralAccY=0; |
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
202,6 → 203,9 |
#define OPA_OFFSET_STEP 10 |
#endif |
|
#define MAX_DRIFT_NR 32 |
#define MAX_DRIFT_YAW 8 |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
300,11 → 304,37 |
BoatNeutralGier = AdNeutralGier; |
SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
|
SetParamWord(PID_GYRO_NICK,(uint16_t)BoatNeutralNick); |
SetParamWord(PID_GYRO_ROLL,(uint16_t)BoatNeutralRoll); |
SetParamWord(PID_GYRO_YAW,(uint16_t)BoatNeutralGier); |
|
SetParamWord(EE_LAST_GYRO_NICK,(uint16_t)BoatNeutralNick); |
SetParamWord(EE_LAST_GYRO_ROLL,(uint16_t)BoatNeutralRoll); |
SetParamWord(EE_LAST_GYRO_YAW,(uint16_t)BoatNeutralGier); |
} |
|
void StoreLastDriftcompensation(void) |
{ |
int last_nick,last_roll,last_yaw; |
|
last_nick = AdNeutralNick; |
last_roll = AdNeutralRoll; |
last_yaw = AdNeutralGier; |
|
if(last_nick > BoatNeutralNick + MAX_DRIFT_NR) last_yaw = BoatNeutralNick + MAX_DRIFT_NR; |
if(last_nick < BoatNeutralNick - MAX_DRIFT_NR) last_yaw = BoatNeutralNick - MAX_DRIFT_NR; |
if(last_roll > BoatNeutralRoll + MAX_DRIFT_NR) last_roll = BoatNeutralRoll + MAX_DRIFT_NR; |
if(last_roll < BoatNeutralRoll - MAX_DRIFT_NR) last_roll = BoatNeutralRoll - MAX_DRIFT_NR; |
if(last_yaw > BoatNeutralGier + MAX_DRIFT_YAW) last_yaw = BoatNeutralGier + MAX_DRIFT_YAW; |
if(last_yaw < BoatNeutralGier - MAX_DRIFT_YAW) last_yaw = BoatNeutralGier - MAX_DRIFT_YAW; |
|
SetParamWord(EE_LAST_GYRO_NICK,(uint16_t)last_nick); |
SetParamWord(EE_LAST_GYRO_ROLL,(uint16_t)last_roll); |
SetParamWord(EE_LAST_GYRO_YAW,(uint16_t)last_yaw); |
} |
|
|
//############################################################################ |
// Nullwerte ermitteln |
// Parameter: 0 -> after switch on (ignore ACC-Z fault) |
362,6 → 392,7 |
AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
//AdNeutralGier -= 20; |
|
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
381,7 → 412,7 |
// strange settings? |
if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
{ |
printf("\n\rACC not calibrated!\r\n"); |
printf("\r\nACC not calibrated!\r\n"); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
VersionInfo.HardwareError[1] |= FC_ERROR1_ACC_NOT_CAL; |
392,7 → 423,17 |
BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK); |
BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL); |
BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW); |
LastFlightNeutralNick = (int16_t)GetParamWord(EE_LAST_GYRO_NICK); |
LastFlightNeutralRoll = (int16_t)GetParamWord(EE_LAST_GYRO_ROLL); |
LastFlightNeutralGier = (int16_t)GetParamWord(EE_LAST_GYRO_YAW); |
//DebugOut.Analog[16] = LastFlightNeutralNick; |
//DebugOut.Analog[17] = LastFlightNeutralRoll; |
//DebugOut.Analog[18] = LastFlightNeutralGier; |
|
if(abs(BoatNeutralNick - LastFlightNeutralNick) > 200) LastFlightNeutralNick = BoatNeutralNick; |
if(abs(BoatNeutralRoll - LastFlightNeutralRoll) > 200) LastFlightNeutralRoll = BoatNeutralRoll; |
if(abs(BoatNeutralGier - LastFlightNeutralGier) > 50) LastFlightNeutralGier = BoatNeutralGier; |
|
if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom |
{ |
// strange settings? |
400,7 → 441,7 |
|| ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16)) |
|| ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2))) |
{ |
printf("\n\rGyro calibration data not valid\r\n"); |
printf("\r\nGyro calibration data not valid\r\n"); |
sucess = 0; |
FC_StatusFlags3 &= ~FC_STATUS3_BOAT; |
} |
412,6 → 453,22 |
} |
} |
} |
|
// ist it within the Boat-Values? |
if(abs((AdNeutralGier - BoatNeutralGier) > MAX_DRIFT_YAW) || abs((AdNeutralNick - BoatNeutralNick) > MAX_DRIFT_NR) || abs((AdNeutralRoll - BoatNeutralRoll) > MAX_DRIFT_NR)) |
{ |
sucess = 0; |
if(AdjustmentMode == 1) SpeakHoTT = SPEAK_ERR_CALIBARTION; // calibration before start |
AdNeutralNick = BoatNeutralNick; |
AdNeutralRoll = BoatNeutralRoll; |
AdNeutralGier = BoatNeutralGier; |
} |
|
// make average from these three values |
AdNeutralGier = (BoatNeutralGier + LastFlightNeutralGier + AdNeutralGier) / 3; |
AdNeutralNick = (BoatNeutralNick + LastFlightNeutralNick + AdNeutralNick) / 3; |
AdNeutralRoll = (BoatNeutralRoll + LastFlightNeutralRoll + AdNeutralRoll) / 3; |
|
EEAR = EE_DUMMY; // Set the EEPROM Address pointer to an unused space |
MesswertNick = 0; |
MesswertRoll = 0; |
1235,6 → 1292,7 |
{ |
FC_StatusFlags3 |= FC_STATUS3_MOTORS_STOPPED_BY_RC; // that informs the slave to disarm the Motors |
Delete_Stoppflag_Timer = 2; // 1-2 seconds |
if(!NC_ErrorCode && modell_fliegt > 60 * 500 && MotorenEin) StoreLastDriftcompensation(); |
MotorenEin = 0; |
delay_ausschalten = 0; |
modell_fliegt = 0; |
1518,6 → 1576,13 |
if(DriftRoll <-3000) { DriftRoll = 0; AdNeutralRoll--;} |
if(GierGyroFehler > 3500) { GierGyroFehler = 0; AdNeutralGier++; } |
if(GierGyroFehler <-3500) { GierGyroFehler = 0; AdNeutralGier--; } |
|
if(AdNeutralNick > BoatNeutralNick + MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralNick + MAX_DRIFT_NR; |
if(AdNeutralNick < BoatNeutralNick - MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralNick - MAX_DRIFT_NR; |
if(AdNeutralRoll > BoatNeutralRoll + MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralRoll + MAX_DRIFT_NR; |
if(AdNeutralRoll < BoatNeutralRoll - MAX_DRIFT_NR) AdNeutralRoll = BoatNeutralRoll - MAX_DRIFT_NR; |
if(AdNeutralGier > BoatNeutralGier + MAX_DRIFT_YAW) AdNeutralGier = BoatNeutralGier + MAX_DRIFT_YAW; |
if(AdNeutralGier < BoatNeutralGier - MAX_DRIFT_YAW) AdNeutralGier = BoatNeutralGier - MAX_DRIFT_YAW; |
} |
else |
{ |