Merge branch 'betaflight' into ready_to_merge
# Conflicts: # src/main/io/serial_msp.c
This commit is contained in:
commit
c889f00d26
|
@ -831,7 +831,7 @@ void mixTable(void)
|
||||||
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
|
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
|
||||||
}
|
}
|
||||||
// Get the maximum correction by setting offset to center
|
// Get the maximum correction by setting offset to center
|
||||||
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
|
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
|
||||||
} else {
|
} else {
|
||||||
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
|
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
|
||||||
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
|
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
|
||||||
|
|
|
@ -174,13 +174,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
|
||||||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||||
// -----calculate scaled error.AngleRates
|
// -----calculate scaled error.AngleRates
|
||||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||||
RateError = (angleRate[axis] - gyroRate) * errorLimiter;
|
RateError = angleRate[axis] - gyroRate;
|
||||||
|
|
||||||
// Smoothed Error for Derivative
|
// Smoothed Error for Derivative
|
||||||
if (flightModeFlags && axis != YAW) {
|
if (flightModeFlags && axis != YAW) {
|
||||||
RateErrorSmooth = RateError;
|
RateErrorSmooth = RateError;
|
||||||
} else {
|
} else {
|
||||||
RateErrorSmooth = (angleRateSmooth[axis] - gyroRate) * errorLimiter;
|
RateErrorSmooth = angleRateSmooth[axis] - gyroRate;
|
||||||
}
|
}
|
||||||
|
|
||||||
// -----calculate P component
|
// -----calculate P component
|
||||||
|
@ -194,7 +194,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
|
||||||
// -----calculate I component.
|
// -----calculate I component.
|
||||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
|
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
|
||||||
|
|
||||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
|
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f);
|
||||||
|
|
||||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||||
|
@ -299,13 +299,13 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl
|
||||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||||
gyroRate = gyroADC[axis] / 4;
|
gyroRate = gyroADC[axis] / 4;
|
||||||
|
|
||||||
RateError = (AngleRateTmp - gyroRate) * errorLimiter;
|
RateError = AngleRateTmp - gyroRate;
|
||||||
|
|
||||||
// Smoothed Error for Derivative
|
// Smoothed Error for Derivative
|
||||||
if (flightModeFlags && axis != YAW) {
|
if (flightModeFlags && axis != YAW) {
|
||||||
RateErrorSmooth = RateError;
|
RateErrorSmooth = RateError;
|
||||||
} else {
|
} else {
|
||||||
RateErrorSmooth = (AngleRateTmpSmooth - gyroRate) * errorLimiter;
|
RateErrorSmooth = AngleRateTmpSmooth - gyroRate;
|
||||||
}
|
}
|
||||||
|
|
||||||
// -----calculate P component
|
// -----calculate P component
|
||||||
|
@ -323,7 +323,9 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl
|
||||||
// is normalized to cycle time = 2048.
|
// is normalized to cycle time = 2048.
|
||||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
|
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
|
||||||
|
|
||||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
|
int32_t rateErrorLimited = errorLimiter * RateError;
|
||||||
|
|
||||||
|
errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI;
|
||||||
|
|
||||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||||
|
|
|
@ -1258,9 +1258,10 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
serialize8(masterConfig.sensorAlignmentConfig.mag_align);
|
serialize8(masterConfig.sensorAlignmentConfig.mag_align);
|
||||||
break;
|
break;
|
||||||
case MSP_PID_ADVANCED_CONFIG :
|
case MSP_PID_ADVANCED_CONFIG :
|
||||||
headSerialReply(5);
|
headSerialReply(6);
|
||||||
serialize8(masterConfig.gyro_sync_denom);
|
serialize8(masterConfig.gyro_sync_denom);
|
||||||
serialize8(masterConfig.pid_process_denom);
|
serialize8(masterConfig.pid_process_denom);
|
||||||
|
serialize8(0);
|
||||||
serialize8(masterConfig.motor_pwm_protocol);
|
serialize8(masterConfig.motor_pwm_protocol);
|
||||||
serialize16(masterConfig.motor_pwm_rate);
|
serialize16(masterConfig.motor_pwm_rate);
|
||||||
break;
|
break;
|
||||||
|
@ -1712,26 +1713,6 @@ static bool processInCommand(void)
|
||||||
|
|
||||||
case MSP_SET_BF_CONFIG:
|
case MSP_SET_BF_CONFIG:
|
||||||
|
|
||||||
case MSP_SET_PID_ADVANCED_CONFIG :
|
|
||||||
masterConfig.gyro_sync_denom = read8();
|
|
||||||
masterConfig.pid_process_denom = read8();
|
|
||||||
masterConfig.motor_pwm_protocol = read8();
|
|
||||||
masterConfig.motor_pwm_rate = read16();
|
|
||||||
break;
|
|
||||||
case MSP_SET_FILTER_CONFIG :
|
|
||||||
masterConfig.gyro_soft_lpf_hz = read8();
|
|
||||||
currentProfile->pidProfile.dterm_lpf_hz = read16();
|
|
||||||
currentProfile->pidProfile.yaw_lpf_hz = read16();
|
|
||||||
break;
|
|
||||||
case MSP_SET_ADVANCED_TUNING:
|
|
||||||
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
|
|
||||||
currentProfile->pidProfile.yawItermIgnoreRate = read16();
|
|
||||||
currentProfile->pidProfile.yaw_p_limit = read16();
|
|
||||||
break;
|
|
||||||
case MSP_SET_TEMPORARY_COMMANDS:
|
|
||||||
currentControlRateProfile->rcYawRate8 = read8();
|
|
||||||
break;
|
|
||||||
|
|
||||||
#ifdef USE_QUAD_MIXER_ONLY
|
#ifdef USE_QUAD_MIXER_ONLY
|
||||||
read8(); // mixerMode ignored
|
read8(); // mixerMode ignored
|
||||||
#else
|
#else
|
||||||
|
@ -1846,6 +1827,27 @@ static bool processInCommand(void)
|
||||||
// proceed as usual with MSP commands
|
// proceed as usual with MSP commands
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
case MSP_SET_PID_ADVANCED_CONFIG :
|
||||||
|
masterConfig.gyro_sync_denom = read8();
|
||||||
|
masterConfig.pid_process_denom = read8();
|
||||||
|
read8();
|
||||||
|
masterConfig.motor_pwm_protocol = read8();
|
||||||
|
masterConfig.motor_pwm_rate = read16();
|
||||||
|
break;
|
||||||
|
case MSP_SET_FILTER_CONFIG :
|
||||||
|
masterConfig.gyro_soft_lpf_hz = read8();
|
||||||
|
currentProfile->pidProfile.dterm_lpf_hz = read16();
|
||||||
|
currentProfile->pidProfile.yaw_lpf_hz = read16();
|
||||||
|
break;
|
||||||
|
case MSP_SET_ADVANCED_TUNING:
|
||||||
|
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
|
||||||
|
currentProfile->pidProfile.yawItermIgnoreRate = read16();
|
||||||
|
currentProfile->pidProfile.yaw_p_limit = read16();
|
||||||
|
break;
|
||||||
|
case MSP_SET_TEMPORARY_COMMANDS:
|
||||||
|
currentControlRateProfile->rcYawRate8 = read8();
|
||||||
|
break;
|
||||||
default:
|
default:
|
||||||
// we do not know how to handle the (valid) message, indicate error MSP $M!
|
// we do not know how to handle the (valid) message, indicate error MSP $M!
|
||||||
return false;
|
return false;
|
||||||
|
|
Loading…
Reference in New Issue