rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

First draft of better horizon mode

This commit is contained in:
Ben Hitchcock 2015-01-04 22:46:01 +08:00
parent 035aff02d3
commit 6b7c9facd3
5 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/config/config.c
View File

@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D_f[YAW] = 0.05f;
pidProfile->A_level = 5.0f;
pidProfile->H_level = 3.0f;
pidProfile->H_sensitivity = 10.0f;
}
#ifdef GPS

21
src/main/flight/flight.c
View File

@ -101,9 +101,28 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
float delta, deltaSum;
float dT;
int axis;
float horizonLevelStrength = 1;
dT = (float)cycleTime * 0.000001f;
if (FLIGHT_MODE(HORIZON_MODE)) {
if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){
axis = FD_ROLL;
}
else {
axis = FD_PITCH;
}
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->H_sensitivity == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1);
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
@ -134,7 +153,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level;
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
}
}
}

1
src/main/flight/flight.h
View File

@ -42,6 +42,7 @@ typedef struct pidProfile_s {
float D_f[3];
float A_level;
float H_level;
float H_sensitivity;
} pidProfile_t;
typedef enum {

1
src/main/io/serial_cli.c
View File

@ -392,6 +392,7 @@ const clivalue_t valueTable[] = {
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 },
{ "sensitivity_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 },

4
src/main/io/serial_msp.c
View File

@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP)
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
serialize8(0);
serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250));
} else {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
@ -1165,7 +1165,7 @@ static bool processInCommand(void)
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
read8();
currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f;
} else {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();