47,8 → 47,7 |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
82,7 → 81,6 |
|
float GyroFaktor; |
float IntegralFaktor; |
|
volatile int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
116,6 → 114,7 |
unsigned char Parameter_AchsKopplung1 = 0; |
unsigned char Parameter_AchsGegenKopplung1 = 0; |
struct mk_param_struct EE_Parameter; |
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
|
void Piep(unsigned char Anzahl) |
{ |
173,6 → 172,7 |
beeptime = 50; |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
ExternHoehenValue = 0; |
} |
|
//############################################################################ |
267,6 → 267,7 |
ANALOG_ON; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[11] = MesswertRoll; |
//DebugOut.Analog[11] = AdWertNick; |
|
Integral_Gier = Mess_Integral_Gier; |
IntegralNick = Mess_IntegralNick; |
541,6 → 542,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || Notlandung) |
{ |
int tmp_int; |
ParameterZuordnung(); |
StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
550,6 → 552,24 |
GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
|
#define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8) |
if(DubWiseKeys[1]) beeptime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8; |
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else |
if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; |
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
|
if(DubWiseKeys[0] & 8) ExternStickGier = 50;else |
if(DubWiseKeys[0] & 4) ExternStickGier =-50;else ExternStickGier = 0; |
if(DubWiseKeys[0] & 2) ExternHoehenValue++; |
if(DubWiseKeys[0] & 16) ExternHoehenValue--; |
|
StickNick += ExternStickNick / 8; |
StickRoll += ExternStickRoll / 8; |
StickGier += ExternStickGier; |
|
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
674,7 → 694,7 |
tmp_long /= 16; |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long2 /= 16; |
#define AUSGLEICH 32 |
#define AUSGLEICH 32 //(Parameter_UserParam1 / 2) |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
696,13 → 716,13 |
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
#define MAX_I 1//(Poti2/10) |
#define MAX_I 0//(Poti2/10) |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000) |
/* if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000) |
{ |
tmp_long = IntegralFehlerNick / 700L; |
tmp_long = IntegralFehlerNick / 256L; |
if(tmp_long > MAX_I) tmp_long = MAX_I; |
if(tmp_long <-MAX_I) tmp_long =-MAX_I; |
I_LageNick = tmp_long; |
711,12 → 731,13 |
{ |
I_LageNick /= 2; |
} |
*/ |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000) |
/* if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000) |
{ |
tmp_long2 = IntegralFehlerRoll / 700L; |
tmp_long2 = IntegralFehlerRoll / 256L; |
if(tmp_long2 > MAX_I) tmp_long2 = MAX_I; |
if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I; |
I_LageRoll = tmp_long2; |
725,11 → 746,7 |
{ |
I_LageRoll /=2; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MittelIntegralNick_Alt = MittelIntegralNick; |
MittelIntegralRoll_Alt = MittelIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
Mess_IntegralNick -= ausgleichNick; |
Mess_IntegralRoll -= ausgleichRoll; |
|
738,8 → 755,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MittelIntegralNick2 /= ABGLEICH_ANZAHL; |
MittelIntegralRoll2 /= ABGLEICH_ANZAHL; |
tmp_long = (long)(MittelIntegralNick2 - (long)IntegralAccNick); |
tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll); |
// tmp_long = (long)(MittelIntegralNick2 - (long)IntegralAccNick); |
// tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll); |
tmp_long = IntegralNick2 - IntegralNick; |
tmp_long2 = IntegralRoll2 - IntegralRoll; |
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26; |
|
IntegralFehlerNick = tmp_long; |
761,19 → 780,21 |
DebugOut.Analog[29] = ausgleichRoll; |
DebugOut.Analog[30] = I_LageRoll * 10; |
|
#define FEHLER_LIMIT (ABGLEICH_ANZAHL/2) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4) |
|
#define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) |
#define BEWEGUNGS_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT) |
{ |
if(IntegralFehlerNick > FEHLER_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; |
ausgleichNick = IntegralFehlerNick / 12; |
ausgleichNick = IntegralFehlerNick / 8; |
if(ausgleichNick > 5000) ausgleichNick = 5000; |
// Mess_IntegralNick -= ausgleichNick; |
Mess_IntegralNick -= ausgleichNick; |
} |
else last_n_p = 1; |
} else last_n_p = 0; |
782,28 → 803,33 |
if(last_n_n) |
{ |
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; |
ausgleichNick = IntegralFehlerNick / 12; |
ausgleichNick = IntegralFehlerNick / 8; |
if(ausgleichNick < -5000) ausgleichNick = -5000; |
// Mess_IntegralNick -= ausgleichNick; |
Mess_IntegralNick -= ausgleichNick; |
} |
else last_n_n = 1; |
} else last_n_n = 0; |
|
if(cnt > 5) cnt = 5; |
} else cnt = 0; |
if(cnt > 4) cnt = 4; |
// if(cnt > Poti2 / 40) cnt = Poti2 / 40; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
|
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerRoll) / 4096; |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
|
ausgleichRoll = 0; |
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT) |
{ |
if(IntegralFehlerRoll > FEHLER_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; |
ausgleichRoll = IntegralFehlerRoll / 12; |
ausgleichRoll = IntegralFehlerRoll / 8; |
if(ausgleichRoll > 5000) ausgleichRoll = 5000; |
// Mess_IntegralRoll -= ausgleichRoll; |
Mess_IntegralRoll -= ausgleichRoll; |
// beeptime = 50; |
} |
else last_r_p = 1; |
} else last_r_p = 0; |
812,20 → 838,32 |
if(last_r_n) |
{ |
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; |
ausgleichRoll = IntegralFehlerRoll / 12; |
ausgleichRoll = IntegralFehlerRoll / 8; |
if(ausgleichRoll < -5000) ausgleichRoll = -5000; |
// Mess_IntegralRoll -= ausgleichRoll; |
Mess_IntegralRoll -= ausgleichRoll; |
// beeptime = 50; |
} |
else last_r_n = 1; |
} else last_r_n = 0; |
} else |
{ |
beeptime = 50; |
cnt = 0; |
} |
DebugOut.Analog[27] = ausgleichRoll; |
if(cnt > 4) cnt = 4; |
// if(cnt > Poti2 / 40) cnt = Poti2 / 40; |
|
DebugOut.Analog[27] = ausgleichRoll; |
if(cnt > 5) cnt = 5; |
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
|
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MittelIntegralNick_Alt = MittelIntegralNick; |
MittelIntegralRoll_Alt = MittelIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
963,7 → 1001,7 |
} |
else |
{ |
SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20; |
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; |
HoehenReglerAktiv = 1; |
} |
|