Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

ff from interpolated setpoint

Browse source
This commit is contained in:
Thorsten Laux 2019-02-22 08:23:45 +01:00
parent 3ba5f7e819
commit 91ad2498ff
13 changed files with 319 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -90,6 +90,7 @@ COMMON_SRC = \
flight/gps_rescue.c \
flight/gyroanalyse.c \
flight/imu.c \
flight/interpolated_setpoint.c \
flight/mixer.c \
flight/mixer_tricopter.c \
flight/pid.c \

2
src/main/build/debug.c
View file

@ -93,4 +93,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"BARO",
"GPS_RESCUE_THROTTLE_PID",
"DYN_IDLE",
"FF_LIMIT",
"FF_INTERPOLATED",
};

2
src/main/build/debug.h
View file

@ -109,6 +109,8 @@ typedef enum {
DEBUG_BARO,
DEBUG_GPS_RESCUE_THROTTLE_PID,
DEBUG_DYN_IDLE,
DEBUG_FF_LIMIT,
DEBUG_FF_INTERPOLATED,
DEBUG_COUNT
} debugType_e;

7
src/main/cli/settings.c
View file

@ -1060,6 +1060,13 @@ const clivalue_t valueTable[] = {
#ifdef USE_AIRMODE_LPF
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
#endif
#ifdef USE_INTERPOLATED_SP
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
{ "ff_spread", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 50}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spread) },
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
{ "ff_lookahead_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_lookahead_limit) },
#endif
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
#ifdef USE_DYN_IDLE
{ "idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, idle_min_rpm) },

44
src/main/fc/rc.c
View file

@ -53,6 +53,12 @@
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
#ifdef USE_INTERPOLATED_SP
// Setpoint in degrees/sec before RC-Smoothing is applied
static float rawSetpoint[XYZ_AXIS_COUNT];
// Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
static float rawDeflection[XYZ_AXIS_COUNT];
#endif
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
@ -60,6 +66,7 @@ static applyRatesFn *applyRates;
uint16_t currentRxRefreshRate;
FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber;
enum {
ROLL_FLAG = 1 << ROLL,
@ -82,6 +89,11 @@ enum {
static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER
uint32_t getRcFrameNumber()
{
return rcFrameNumber;
}
float getSetpointRate(int axis)
{
return setpointRate[axis];
@ -102,6 +114,18 @@ float getThrottlePIDAttenuation(void)
return throttlePIDAttenuation;
}
#ifdef USE_INTERPOLATED_SP
float getRawSetpoint(int axis)
{
return rawSetpoint[axis];
}
float getRawDeflection(int axis)
{
return rawDeflection[axis];
}
#endif
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
@ -159,6 +183,11 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
return kissAngle;
}
float applyCurve(int axis, float deflection)
{
return applyRates(axis, deflection, fabsf(deflection));
}
static void calculateSetpointRate(int axis)
{
float angleRate;
@ -561,10 +590,25 @@ FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
if (isRXDataNew) {
rcFrameNumber++;
}
if (isRXDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}
#ifdef USE_INTERPOLATED_SP
if (isRXDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
const float rcCommandf = rcCommand[i] / 500.0f;
const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
rawDeflection[i] = rcCommandf;
}
}
#endif
switch (rxConfig()->rc_smoothing_type) {
#ifdef USE_RC_SMOOTHING_FILTER
case RC_SMOOTHING_TYPE_FILTER:

4
src/main/fc/rc.h
View file

@ -45,3 +45,7 @@ bool rcSmoothingIsEnabled(void);
rcSmoothingFilter_t *getRcSmoothingData(void);
bool rcSmoothingAutoCalculate(void);
bool rcSmoothingInitializationComplete(void);
float getRawSetpoint(int axis);
float getRawDeflection(int axis);
float applyCurve(int axis, float deflection);
uint32_t getRcFrameNumber();

151
src/main/flight/interpolated_setpoint.c Normal file
View file

@ -0,0 +1,151 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_INTERPOLATED_SP
#include <math.h>
#include "build/debug.h"
#include "common/maths.h"
#include "fc/rc.h"
#include "flight/interpolated_setpoint.h"
static float projectedSetpoint[XYZ_AXIS_COUNT];
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
static float prevRawSetpoint[XYZ_AXIS_COUNT];
static float prevRawDeflection[XYZ_AXIS_COUNT];
static uint16_t interpolationSteps[XYZ_AXIS_COUNT];
static float setpointChangePerIteration[XYZ_AXIS_COUNT];
static float deflectionChangePerIteration[XYZ_AXIS_COUNT];
static float setpointReservoir[XYZ_AXIS_COUNT];
static float deflectionReservoir[XYZ_AXIS_COUNT];
// Configuration
static float ffLookaheadLimit;
static float ffSpread;
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
static float ffMaxRate[XYZ_AXIS_COUNT];
void interpolatedSpInit(const pidProfile_t *pidProfile) {
const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
ffLookaheadLimit = pidProfile->ff_lookahead_limit * 0.0001f;
ffSpread = pidProfile->ff_spread;
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
ffMaxRate[i] = applyCurve(i, 1.0f);
ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
}
}
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame) {
const float rawSetpoint = getRawSetpoint(axis);
const float rawDeflection = getRawDeflection(axis);
float pidSetpointDelta = 0.0f;
static int iterationsSinceLastUpdate[XYZ_AXIS_COUNT];
if (newRcFrame) {
setpointReservoir[axis] -= iterationsSinceLastUpdate[axis] * setpointChangePerIteration[axis];
deflectionReservoir[axis] -= iterationsSinceLastUpdate[axis] * deflectionChangePerIteration[axis];
iterationsSinceLastUpdate[axis] = 0;
// get the number of interpolation steps either dynamically based on RX refresh rate
// or manually based on ffSpread configuration property
if (ffSpread) {
interpolationSteps[axis] = (uint16_t) ((ffSpread + 1.0f) * 0.001f * pidFrequency);
} else {
interpolationSteps[axis] = (uint16_t) ((currentRxRefreshRate + 1000) * pidFrequency * 1e-6f + 0.5f);
}
// interpolate stick deflection
deflectionReservoir[axis] += rawDeflection - prevRawDeflection[axis];
deflectionChangePerIteration[axis] = deflectionReservoir[axis] / interpolationSteps[axis];
const float projectedStickPos =
rawDeflection + deflectionChangePerIteration[axis] * pidFrequency * ffLookaheadLimit;
projectedSetpoint[axis] = applyCurve(axis, projectedStickPos);
prevRawDeflection[axis] = rawDeflection;
// apply linear interpolation on setpoint
setpointReservoir[axis] += rawSetpoint - prevRawSetpoint[axis];
const float ffBoostFactor = pidGetFfBoostFactor();
if (ffBoostFactor != 0.0f) {
const float speed = rawSetpoint - prevRawSetpoint[axis];
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(3.0f * speed) > fabsf(prevSetpointSpeed[axis])) {
const float setpointAcc = speed - prevSetpointSpeed[axis];
setpointReservoir[axis] += ffBoostFactor * setpointAcc;
}
prevSetpointSpeed[axis] = speed;
}
setpointChangePerIteration[axis] = setpointReservoir[axis] / interpolationSteps[axis];
prevRawSetpoint[axis] = rawSetpoint;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, rawDeflection * 100);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, projectedStickPos * 100);
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, projectedSetpoint[axis]);
}
}
if (iterationsSinceLastUpdate[axis] < interpolationSteps[axis]) {
iterationsSinceLastUpdate[axis]++;
pidSetpointDelta = setpointChangePerIteration[axis];
}
return pidSetpointDelta;
}
FAST_CODE_NOINLINE float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) {
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
}
if (ffLookaheadLimit) {
const float limit = fabsf((projectedSetpoint[axis] - prevRawSetpoint[axis]) * Kp);
value = constrainf(value, -limit, limit);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, projectedSetpoint[axis]);
}
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
}
if (ffMaxRateLimit[axis]) {
if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
} else {
value = 0;
}
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
}
return value;
}
bool shouldApplyFFLimits(int axis)
{
return ffLookaheadLimit != 0.0f || ffMaxRateLimit[axis] != 0.0f;
}
#endif

31
src/main/flight/interpolated_setpoint.h Normal file
View file

@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "common/axis.h"
#include "flight/pid.h"
void interpolatedSpInit(const pidProfile_t *pidProfile);
float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame);
float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint);
bool shouldApplyFFLimits(int axis);

63
src/main/flight/pid.c
View file

@ -49,6 +49,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/rpm_filter.h"
#include "flight/interpolated_setpoint.h"
#include "io/gps.h"
@ -211,6 +212,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
.idle_p = 50,
.idle_pid_limit = 200,
.idle_max_increase = 150,
.ff_interpolate_sp = 0,
.ff_spread = 0,
.ff_max_rate_limit = 0,
.ff_lookahead_limit = 0,
.ff_boost = 15,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;
@ -285,6 +291,7 @@ static FAST_RAM_ZERO_INIT float acLimit;
static FAST_RAM_ZERO_INIT float acErrorLimit;
static FAST_RAM_ZERO_INIT float acCutoff;
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float oldSetpointCorrection[XYZ_AXIS_COUNT];
#endif
#if defined(USE_D_MIN)
@ -307,6 +314,14 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
static FAST_RAM_ZERO_INIT float ffBoostFactor;
float pidGetFfBoostFactor()
{
return ffBoostFactor;
}
void pidInitFilters(const pidProfile_t *pidProfile)
{
STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
@ -443,6 +458,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -565,6 +582,10 @@ static FAST_RAM_ZERO_INIT float dMinGyroGain;
static FAST_RAM_ZERO_INIT float dMinSetpointGain;
#endif
#ifdef USE_INTERPOLATED_SP
static FAST_RAM_ZERO_INIT bool ffFromInterpolatedSetpoint;
#endif
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedForwardTransition == 0) {
@ -708,6 +729,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#if defined(USE_AIRMODE_LPF)
airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
#ifdef USE_INTERPOLATED_SP
ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
interpolatedSpInit(pidProfile);
#endif
}
void pidInit(const pidProfile_t *pidProfile)
@ -1212,6 +1237,9 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
#ifdef USE_INTERPOLATED_SP
static FAST_RAM_ZERO_INIT uint32_t lastFrameNumber;
#endif
#if defined(USE_ACC)
static timeUs_t levelModeStartTimeUs = 0;
@ -1286,6 +1314,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate();
#endif
#ifdef USE_INTERPOLATED_SP
bool newRcFrame = false;
if (lastFrameNumber != getRcFrameNumber()) {
lastFrameNumber = getRcFrameNumber();
newRcFrame = true;
}
#endif
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@ -1336,6 +1371,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float previousIterm = pidData[axis].I;
float itermErrorRate = errorRate;
float uncorrectedSetpoint = currentPidSetpoint;
#if defined(USE_ITERM_RELAX)
if (!launchControlActive && !inCrashRecoveryMode) {
@ -1343,6 +1379,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
errorRate = currentPidSetpoint - gyroRate;
}
#endif
#ifdef USE_ABSOLUTE_CONTROL
float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
#endif
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
@ -1365,9 +1404,18 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate pidSetpointDelta
float pidSetpointDelta = 0;
#ifdef USE_INTERPOLATED_SP
if (ffFromInterpolatedSetpoint) {
pidSetpointDelta = interpolatedSpApply(axis, pidFrequency, newRcFrame);
} else {
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
}
#else
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
#endif
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_RC_SMOOTHING_FILTER
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
#endif // USE_RC_SMOOTHING_FILTER
@ -1416,12 +1464,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
#ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in FF
pidSetpointDelta += setpointCorrection - oldSetpointCorrection[axis];
oldSetpointCorrection[axis] = setpointCorrection;
#endif
// Only enable feedforward for rate mode and if launch control is inactive
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
// no transition if feedForwardTransition == 0
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
float feedForward = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
#ifdef USE_INTERPOLATED_SP
pidData[axis].F = shouldApplyFFLimits(axis) ?
applyFFLimit(axis, feedForward, pidCoefficient[axis].Kp, currentPidSetpoint) : feedForward;
#else
pidData[axis].F = feedForward;
#endif
} else {
pidData[axis].F = 0;
}

8
src/main/flight/pid.h
View file

@ -170,6 +170,7 @@ typedef struct pidProfile_s {
uint8_t motor_output_limit; // Upper limit of the motor output (percent)
int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used
uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
uint8_t ff_boost; // amount of high-pass filtered FF to add to FF, 100 means 100% added
char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
uint8_t idle_min_rpm; // minimum motor speed enforced by integrating p controller
@ -178,7 +179,10 @@ typedef struct pidProfile_s {
uint8_t idle_pid_limit; // max P
uint8_t idle_max_increase; // max integrated correction
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
uint8_t ff_spread; // Spread ff out over at least min spread ms
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
uint8_t ff_lookahead_limit; // FF stick extrapolation lookahead period in ms
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@ -255,4 +259,4 @@ void pidSetItermReset(bool enabled);
float pidGetPreviousSetpoint(int axis);
float pidGetDT();
float pidGetPidFrequency();
float pidGetFfBoostFactor();

1
src/main/target/common_pre.h
View file

@ -334,4 +334,5 @@
#define USE_PERSISTENT_STATS
#define USE_PROFILE_NAMES
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_INTERPOLATED_SP
#endif

6
src/test/unit/pid_unittest.cc
View file

@ -80,6 +80,12 @@ extern "C" {
void beeperConfirmationBeeps(uint8_t) { }
bool isLaunchControlActive(void) {return unitLaunchControlActive; }
void disarm(void) { }
float applyFFLimit(int axis, float value, float Kp, float currentPidSetpoint) {
UNUSED(axis);
UNUSED(Kp);
UNUSED(currentPidSetpoint);
return value;
}
}
pidProfile_t *pidProfile;