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

Merge remote-tracking branch 'borisbstyle/betaflight' into betaflight

This commit is contained in:
Gary Keeble 2016-06-01 21:36:04 +01:00
parent 89d63a9b95 c45475b8af
commit 0291a3e066
4 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -817,6 +817,7 @@ void mixTable(void)
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
for (i = 0; i < motorCount; i++) {
@ -827,6 +828,7 @@ void mixTable(void)
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
} else {
motorLimitReached = false;
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
@ -857,11 +859,15 @@ void mixTable(void)
// Experimental Code. Anti Desync feature for ESC's
if (escAndServoConfig->escDesyncProtection) {
const int16_t maxThrottleStep = escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime);
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000);
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
}
}
}

2
src/main/flight/mixer.h
View File

@ -189,7 +189,7 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
bool motorLimitReached;
struct escAndServoConfig_s;
struct rxConfig_s;

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

@ -49,6 +49,7 @@
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern bool motorLimitReached;
uint32_t targetPidLooptime;
int16_t axisPID[3];
@ -60,9 +61,9 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3];
static float errorGyroIf[3], errorGyroIfLimit[3];;
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
@ -173,7 +174,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = calculateRate(axis, controlRateConfig);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
@ -215,6 +216,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// 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
ITerm = errorGyroIf[axis];
@ -292,7 +299,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// -----Get the desired angle rate depending on flight mode
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to max configured inclination
#ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
@ -341,6 +348,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term

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

@ -574,7 +574,7 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 2000 } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently