| 645,20 → 645,54 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!NewPpmData-- || Notlandung) |
| { |
| int tmp_int; |
| static int chanNickPrev = 0; |
| static int chanRollPrev = 0; |
| |
| static int stick_nick,stick_roll; |
| ParameterZuordnung(); |
| stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
| stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
| StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
| // StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
| |
| #define MAX_CHAN_VAL 125L |
| #define COS45 7071L // cos( -45 ) * 10000 |
| |
| stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
| stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
| StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
| long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
| long chanRoll = -PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
| |
| // StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
| int chanNickDiff; |
| int chanRollDiff; |
| |
| // Stick-Koordinatensystem um -45° (rechts) drehen |
| chanNick *= COS45; |
| chanRoll *= COS45; |
| |
| int chanNickTemp = ( chanNick - chanRoll ) / 10000L; |
| int chanRollTemp = ( chanRoll + chanNick ) / 10000L; |
| |
| chanNick = chanNickTemp; |
| chanRoll = -chanRollTemp; |
| |
| if (chanNick > MAX_CHAN_VAL) |
| chanNick = MAX_CHAN_VAL; |
| if (chanNick < -MAX_CHAN_VAL) |
| chanNick = -MAX_CHAN_VAL; |
| if (chanRoll > MAX_CHAN_VAL) |
| chanRoll = MAX_CHAN_VAL; |
| if (chanRoll < -MAX_CHAN_VAL) |
| chanRoll = -MAX_CHAN_VAL; |
| |
| chanNickDiff = ( ( chanNick - chanNickPrev ) / 3) * 3; |
| chanRollDiff = ( ( chanRoll - chanRollPrev ) / 3) * 3; |
| |
| chanNickPrev = chanNick; |
| chanRollPrev = chanRoll; |
| |
| stick_nick = (stick_nick * 3 + ( (int) chanNick ) * EE_Parameter.Stick_P) / 4; |
| stick_nick += chanNickDiff * EE_Parameter.Stick_D; |
| StickNick = stick_nick - GPS_Nick; |
| |
| stick_roll = (stick_roll * 3 + ( (int) chanRoll ) * EE_Parameter.Stick_P) / 4; |
| stick_roll += chanRollDiff * EE_Parameter.Stick_D; |
| StickRoll = stick_roll - GPS_Roll; |
| |
| StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
| StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
| |