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

Add roll pitch velocity

This commit is contained in:
borisbstyle 2016-08-05 00:55:12 +02:00
parent 06be182e50
commit 38e812a5a5
5 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 142;
static const uint8_t EEPROM_CONF_VERSION = 143;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -246,6 +246,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->ptermSetpointWeight = 75;
pidProfile->dtermSetpointWeight = 120;
pidProfile->pidMaxVelocityYaw = 200;
pidProfile->pidMaxVelocityRollPitch = 0;
pidProfile->toleranceBand = 20;
pidProfile->toleranceBandReduction = 40;
pidProfile->zeroCrossAllowanceCount = 2;

14
src/main/flight/pid.c
View File

@ -133,7 +133,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm;
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate;
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
float delta;
int axis;
float horizonLevelStrength = 1;
@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT();
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
@ -195,12 +196,13 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
}
// Limit abrupt yaw inputs / stops
if (axis == YAW && pidProfile->pidMaxVelocityYaw) {
float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate;
if (ABS(yawCurrentVelocity) > yawMaxVelocity) {
setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity;
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
}
yawPreviousRate = setpointRate[axis];
previousSetpoint[axis] = setpointRate[axis];
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID

3
src/main/flight/pid.h
View File

@ -100,7 +100,8 @@ typedef struct pidProfile_s {
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking)
uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms
uint16_t pidMaxVelocityYaw; // velocity yaw limiter for pid controller deg/sec/ms
uint16_t pidMaxVelocityRollPitch; // velocity roll/pitch limiter for pid controller deg/sec/ms
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

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

@ -773,8 +773,8 @@ const clivalue_t valueTable[] = {
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
#endif
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } },
{ "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } },
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } },
{ "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } },
{ "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } },
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } },
{ "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
@ -833,6 +833,7 @@ const clivalue_t valueTable[] = {
{ "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } },
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } },
{ "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } },
{ "max_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityRollPitch, .config.minmax = {0, 1000 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },

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

@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->pidProfile.toleranceBand);
serialize8(currentProfile->pidProfile.toleranceBandReduction);
serialize8(currentProfile->pidProfile.itermThrottleGain);
serialize16(currentProfile->pidProfile.pidMaxVelocityRollPitch);
serialize16(currentProfile->pidProfile.pidMaxVelocityYaw);
serialize16(1000); // Reserved for future
break;
case MSP_SENSOR_CONFIG:
headSerialReply(3);
@ -1858,8 +1858,8 @@ static bool processInCommand(void)
currentProfile->pidProfile.toleranceBand = read8();
currentProfile->pidProfile.toleranceBandReduction = read8();
currentProfile->pidProfile.itermThrottleGain = read8();
currentProfile->pidProfile.pidMaxVelocityRollPitch = read16();
currentProfile->pidProfile.pidMaxVelocityYaw = read16();
read16(); // reserved for future purposes
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8();