mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
major rc changes ctzsnooze 2021
This commit is contained in:
ctzsnooze
parent
2a5e457603
commit
636d563abe
26 changed files with 377 additions and 546 deletions
379
src/main/fc/rc.c
379
src/main/fc/rc.c
|
|
@ -41,7 +41,7 @@
|
|||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/interpolated_setpoint.h"
|
||||
#include "flight/feedforward.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/pid_init.h"
|
||||
|
||||
|
|
@ -57,14 +57,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];
|
||||
#ifdef USE_FEEDFORWARD
|
||||
static float oldRcCommand[XYZ_AXIS_COUNT];
|
||||
static bool isDuplicate[XYZ_AXIS_COUNT];
|
||||
float rcCommandDelta[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
|
||||
static float rawSetpoint[XYZ_AXIS_COUNT];
|
||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||
static float throttlePIDAttenuation;
|
||||
static bool reverseMotors = false;
|
||||
|
|
@ -74,7 +72,6 @@ static bool isRxDataNew = false;
|
|||
static float rcCommandDivider = 500.0f;
|
||||
static float rcCommandYawDivider = 500.0f;
|
||||
|
||||
FAST_DATA_ZERO_INIT uint8_t interpolationChannels;
|
||||
static FAST_DATA_ZERO_INIT bool newRxDataForFF;
|
||||
|
||||
enum {
|
||||
|
|
@ -93,13 +90,13 @@ enum {
|
|||
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
|
||||
#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms
|
||||
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
|
||||
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled
|
||||
#define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
|
||||
|
||||
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
bool getShouldUpdateFf()
|
||||
// only used in pid.c when interpolated_sp is active to initiate a new FF value
|
||||
bool getShouldUpdateFeedforward()
|
||||
// only used in pid.c, when feedforward is enabled, to initiate a new FF value
|
||||
{
|
||||
const bool updateFf = newRxDataForFF;
|
||||
if (newRxDataForFF == true){
|
||||
|
|
@ -129,7 +126,7 @@ float getThrottlePIDAttenuation(void)
|
|||
return throttlePIDAttenuation;
|
||||
}
|
||||
|
||||
#ifdef USE_INTERPOLATED_SP
|
||||
#ifdef USE_FEEDFORWARD
|
||||
float getRawSetpoint(int axis)
|
||||
{
|
||||
return rawSetpoint[axis];
|
||||
|
|
@ -139,7 +136,6 @@ float getRcCommandDelta(int axis)
|
|||
{
|
||||
return rcCommandDelta[axis];
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#define THROTTLE_LOOKUP_LENGTH 12
|
||||
|
|
@ -240,43 +236,7 @@ float applyCurve(int axis, float deflection)
|
|||
return applyRates(axis, deflection, fabsf(deflection));
|
||||
}
|
||||
|
||||
static void calculateSetpointRate(int axis)
|
||||
{
|
||||
float angleRate;
|
||||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
||||
// If GPS Rescue is active then override the setpointRate used in the
|
||||
// pid controller with the value calculated from the desired heading logic.
|
||||
angleRate = gpsRescueGetYawRate();
|
||||
|
||||
// Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
|
||||
rcDeflection[axis] = 0;
|
||||
rcDeflectionAbs[axis] = 0;
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
// scale rcCommandf to range [-1.0, 1.0]
|
||||
float rcCommandf;
|
||||
if (axis == FD_YAW) {
|
||||
rcCommandf = rcCommand[axis] / rcCommandYawDivider;
|
||||
} else {
|
||||
rcCommandf = rcCommand[axis] / rcCommandDivider;
|
||||
}
|
||||
|
||||
rcDeflection[axis] = rcCommandf;
|
||||
const float rcCommandfAbs = fabsf(rcCommandf);
|
||||
rcDeflectionAbs[axis] = rcCommandfAbs;
|
||||
|
||||
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
|
||||
}
|
||||
// Rate limit from profile (deg/sec)
|
||||
setpointRate[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
|
||||
|
||||
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
|
||||
}
|
||||
|
||||
static void scaleRcCommandToFpvCamAngle(void)
|
||||
static void scaleSetpointToFpvCamAngle(void)
|
||||
{
|
||||
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
|
||||
static uint8_t lastFpvCamAngleDegrees = 0;
|
||||
|
|
@ -323,64 +283,6 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
|
|||
}
|
||||
}
|
||||
|
||||
static FAST_CODE uint8_t processRcInterpolation(void)
|
||||
{
|
||||
static FAST_DATA_ZERO_INIT float rcCommandInterp[4];
|
||||
static FAST_DATA_ZERO_INIT float rcStepSize[4];
|
||||
static FAST_DATA_ZERO_INIT int16_t rcInterpolationStepCount;
|
||||
|
||||
uint16_t rxRefreshRate;
|
||||
uint8_t updatedChannel = 0;
|
||||
|
||||
if (rxConfig()->rcInterpolation) {
|
||||
// Set RC refresh rate for sampling and channels to filter
|
||||
switch (rxConfig()->rcInterpolation) {
|
||||
case RC_SMOOTHING_AUTO:
|
||||
rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps
|
||||
break;
|
||||
case RC_SMOOTHING_MANUAL:
|
||||
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
|
||||
break;
|
||||
case RC_SMOOTHING_OFF:
|
||||
case RC_SMOOTHING_DEFAULT:
|
||||
default:
|
||||
rxRefreshRate = rxGetRefreshRate();
|
||||
}
|
||||
|
||||
if (isRxDataNew && rxRefreshRate > 0) {
|
||||
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
|
||||
|
||||
for (int channel = 0; channel < PRIMARY_CHANNEL_COUNT; channel++) {
|
||||
if ((1 << channel) & interpolationChannels) {
|
||||
rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, 0, lrintf(rcCommand[0]));
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, 1, lrintf(currentRxRefreshRate / 1000));
|
||||
} else {
|
||||
rcInterpolationStepCount--;
|
||||
}
|
||||
|
||||
// Interpolate steps of rcCommand
|
||||
if (rcInterpolationStepCount > 0) {
|
||||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) {
|
||||
rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
|
||||
rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, 2, rcInterpolationStepCount);
|
||||
|
||||
return updatedChannel;
|
||||
|
||||
}
|
||||
|
||||
void updateRcRefreshRate(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t lastRxTimeUs;
|
||||
|
|
@ -400,9 +302,8 @@ uint16_t getCurrentRxRefreshRate(void)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
// Determine a cutoff frequency based on filter type and the calculated
|
||||
// average rx frame time
|
||||
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
|
||||
// Determine a cutoff frequency based on smoothness factor and calculated average rx frame time
|
||||
FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
|
||||
{
|
||||
if (avgRxFrameTimeUs > 0) {
|
||||
const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
|
||||
|
|
@ -426,35 +327,44 @@ static FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
|
|||
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
|
||||
{
|
||||
const float dT = targetPidLooptime * 1e-6f;
|
||||
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
|
||||
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
|
||||
|
||||
if (smoothingData->inputCutoffSetting == 0) {
|
||||
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
|
||||
if (smoothingData->setpointCutoffSetting == 0) {
|
||||
smoothingData->setpointCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
|
||||
}
|
||||
if (smoothingData->throttleCutoffSetting == 0) {
|
||||
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
|
||||
}
|
||||
|
||||
// initialize or update the input filter
|
||||
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||
|
||||
// initialize or update the Setpoint filter
|
||||
if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
|
||||
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
|
||||
} else {
|
||||
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
|
||||
}
|
||||
} else {
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
|
||||
} else {
|
||||
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update or initialize the derivative filter
|
||||
oldCutoff = smoothingData->derivativeCutoffFrequency;
|
||||
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
|
||||
// update or initialize the FF filter
|
||||
oldCutoff = smoothingData->feedforwardCutoffFrequency;
|
||||
if (rcSmoothingData.ffCutoffSetting == 0) {
|
||||
smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint);
|
||||
}
|
||||
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis);
|
||||
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
|
||||
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
|
||||
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
|
||||
} else if (smoothingData->feedforwardCutoffFrequency != oldCutoff) {
|
||||
pidUpdateFeedforwardLpf(smoothingData->feedforwardCutoffFrequency);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -490,17 +400,16 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
|
|||
// examining the rx frame times completely
|
||||
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
|
||||
{
|
||||
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
|
||||
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
// if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs
|
||||
if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.ffCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
||||
static FAST_CODE void processRcSmoothingFilter(void)
|
||||
{
|
||||
uint8_t updatedChannel = 0;
|
||||
static FAST_DATA_ZERO_INIT float lastRxData[4];
|
||||
static FAST_DATA_ZERO_INIT float rxDataToSmooth[4];
|
||||
static FAST_DATA_ZERO_INIT bool initialized;
|
||||
static FAST_DATA_ZERO_INIT timeMs_t validRxFrameTimeMs;
|
||||
static FAST_DATA_ZERO_INIT bool calculateCutoffs;
|
||||
|
|
@ -510,47 +419,42 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
initialized = true;
|
||||
rcSmoothingData.filterInitialized = false;
|
||||
rcSmoothingData.averageFrameTimeUs = 0;
|
||||
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor;
|
||||
rcSmoothingData.autoSmoothnessFactorSetpoint = rxConfig()->rc_smoothing_auto_factor_rpy;
|
||||
rcSmoothingData.autoSmoothnessFactorThrottle = rxConfig()->rc_smoothing_auto_factor_throttle;
|
||||
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
|
||||
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
|
||||
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff;
|
||||
rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff;
|
||||
rcSmoothingData.ffCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff;
|
||||
rcSmoothingResetAccumulation(&rcSmoothingData);
|
||||
|
||||
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
|
||||
|
||||
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known
|
||||
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f));
|
||||
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency
|
||||
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
|
||||
rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting;
|
||||
rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting;
|
||||
if (rcSmoothingData.ffCutoffSetting == 0) {
|
||||
// calculate and use an initial derivative cutoff until the RC interval is known
|
||||
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactorSetpoint / 10.0f));
|
||||
float ffCutoff = RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ * cutoffFactor;
|
||||
rcSmoothingData.feedforwardCutoffFrequency = lrintf(ffCutoff);
|
||||
} else {
|
||||
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
|
||||
rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.ffCutoffSetting;
|
||||
}
|
||||
|
||||
calculateCutoffs = rcSmoothingAutoCalculate();
|
||||
if (rxConfig()->rc_smoothing_mode) {
|
||||
calculateCutoffs = rcSmoothingAutoCalculate();
|
||||
|
||||
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
|
||||
if (!calculateCutoffs) {
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
// if we don't need to calculate cutoffs dynamically then the filters can be initialized now
|
||||
if (!calculateCutoffs) {
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isRxDataNew) {
|
||||
|
||||
// store the new raw channel values
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) {
|
||||
lastRxData[i] = rcCommand[i];
|
||||
}
|
||||
}
|
||||
|
||||
// for dynamically calculated filters we need to examine each rx frame interval
|
||||
// for auto calculated filters we need to examine each rx frame interval
|
||||
if (calculateCutoffs) {
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
int sampleState = 0;
|
||||
|
||||
// If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
|
||||
// If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate
|
||||
// and use that to calculate the filter cutoff frequencies
|
||||
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
|
||||
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) {
|
||||
|
|
@ -582,9 +486,11 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
// accumlate the sample into the average
|
||||
if (accumulateSample) {
|
||||
if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
|
||||
// the required number of samples were collected so set the filter cutoffs
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
// the required number of samples were collected so set the filter cutoffs, but only if smoothing is active
|
||||
if (rxConfig()->rc_smoothing_mode) {
|
||||
rcSmoothingSetFilterCutoffs(&rcSmoothingData);
|
||||
rcSmoothingData.filterInitialized = true;
|
||||
}
|
||||
validRxFrameTimeMs = 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -604,35 +510,38 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active
|
||||
}
|
||||
}
|
||||
// Get new values to be smoothed
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
rxDataToSmooth[i] = i == THROTTLE ? rcCommand[i] : rawSetpoint[i];
|
||||
if (i < THROTTLE) {
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, i, lrintf(rxDataToSmooth[i]));
|
||||
} else {
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, i, ((lrintf(rxDataToSmooth[i])) - 1000));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
|
||||
// after training has completed then log the raw rc channel and the calculated
|
||||
// average rx frame rate that was used to calculate the automatic filter cutoffs
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rcSmoothingData.debugAxis]));
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
|
||||
}
|
||||
|
||||
// each pid loop continue to apply the last received channel value to the filter
|
||||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
} else {
|
||||
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
|
||||
rcCommand[updatedChannel] = lastRxData[updatedChannel];
|
||||
}
|
||||
// each pid loop, apply the last received channel value to the filter, if initialised - thanks @klutvott
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
*dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
|
||||
} else {
|
||||
// If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
|
||||
*dst = rxDataToSmooth[i];
|
||||
}
|
||||
}
|
||||
|
||||
return interpolationChannels;
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
FAST_CODE void processRcCommand(void)
|
||||
{
|
||||
uint8_t updatedChannel;
|
||||
|
||||
if (isRxDataNew) {
|
||||
newRxDataForFF = true;
|
||||
}
|
||||
|
|
@ -641,57 +550,55 @@ FAST_CODE void processRcCommand(void)
|
|||
checkForThrottleErrorResetState(currentRxRefreshRate);
|
||||
}
|
||||
|
||||
#ifdef USE_INTERPOLATED_SP
|
||||
if (isRxDataNew) {
|
||||
for (int i = FD_ROLL; i <= FD_YAW; i++) {
|
||||
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
|
||||
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
|
||||
oldRcCommand[i] = rcCommand[i];
|
||||
float rcCommandf;
|
||||
if (i == FD_YAW) {
|
||||
rcCommandf = rcCommand[i] / rcCommandYawDivider;
|
||||
} else {
|
||||
rcCommandf = rcCommand[i] / rcCommandDivider;
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
|
||||
isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
|
||||
rcCommandDelta[axis] = fabsf(rcCommand[axis] - oldRcCommand[axis]);
|
||||
oldRcCommand[axis] = rcCommand[axis];
|
||||
|
||||
float angleRate;
|
||||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
||||
// If GPS Rescue is active then override the setpointRate used in the
|
||||
// pid controller with the value calculated from the desired heading logic.
|
||||
angleRate = gpsRescueGetYawRate();
|
||||
// Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
|
||||
rcDeflection[axis] = 0;
|
||||
rcDeflectionAbs[axis] = 0;
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
// scale rcCommandf to range [-1.0, 1.0]
|
||||
float rcCommandf;
|
||||
if (axis == FD_YAW) {
|
||||
rcCommandf = rcCommand[axis] / rcCommandYawDivider;
|
||||
} else {
|
||||
rcCommandf = rcCommand[axis] / rcCommandDivider;
|
||||
}
|
||||
|
||||
rcDeflection[axis] = rcCommandf;
|
||||
const float rcCommandfAbs = fabsf(rcCommandf);
|
||||
rcDeflectionAbs[axis] = rcCommandfAbs;
|
||||
|
||||
angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
|
||||
|
||||
}
|
||||
const float rcCommandfAbs = fabsf(rcCommandf);
|
||||
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
|
||||
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
|
||||
}
|
||||
|
||||
switch (rxConfig()->rc_smoothing_type) {
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
case RC_SMOOTHING_TYPE_FILTER:
|
||||
updatedChannel = processRcSmoothingFilter();
|
||||
break;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
case RC_SMOOTHING_TYPE_INTERPOLATION:
|
||||
default:
|
||||
updatedChannel = processRcInterpolation();
|
||||
break;
|
||||
}
|
||||
|
||||
if (isRxDataNew || updatedChannel) {
|
||||
const uint8_t maxUpdatedAxis = isRxDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
|
||||
#if defined(SIMULATOR_BUILD)
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
|
||||
#endif
|
||||
for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
|
||||
#if defined(SIMULATOR_BUILD)
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
calculateSetpointRate(axis);
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
|
||||
|
||||
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
|
||||
// adjust un-filtered setpoint steps to camera angle (mixing Roll and Yaw)
|
||||
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
scaleRcCommandToFpvCamAngle();
|
||||
scaleSetpointToFpvCamAngle();
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
processRcSmoothingFilter();
|
||||
#endif
|
||||
|
||||
isRxDataNew = false;
|
||||
}
|
||||
|
||||
|
|
@ -840,45 +747,13 @@ void initRcProcessing(void)
|
|||
break;
|
||||
}
|
||||
|
||||
interpolationChannels = 0;
|
||||
switch (rxConfig()->rcInterpolationChannels) {
|
||||
case INTERPOLATION_CHANNELS_RPYT:
|
||||
interpolationChannels |= THROTTLE_FLAG;
|
||||
|
||||
FALLTHROUGH;
|
||||
case INTERPOLATION_CHANNELS_RPY:
|
||||
interpolationChannels |= YAW_FLAG;
|
||||
|
||||
FALLTHROUGH;
|
||||
case INTERPOLATION_CHANNELS_RP:
|
||||
interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
|
||||
|
||||
break;
|
||||
case INTERPOLATION_CHANNELS_RPT:
|
||||
interpolationChannels |= ROLL_FLAG | PITCH_FLAG;
|
||||
|
||||
FALLTHROUGH;
|
||||
case INTERPOLATION_CHANNELS_T:
|
||||
interpolationChannels |= THROTTLE_FLAG;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef USE_YAW_SPIN_RECOVERY
|
||||
const int maxYawRate = (int)applyRates(FD_YAW, 1.0f, 1.0f);
|
||||
initYawSpinRecovery(maxYawRate);
|
||||
#endif
|
||||
}
|
||||
|
||||
bool rcSmoothingIsEnabled(void)
|
||||
{
|
||||
return !(
|
||||
#if defined(USE_RC_SMOOTHING_FILTER)
|
||||
rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_INTERPOLATION &&
|
||||
#endif
|
||||
rxConfig()->rcInterpolation == RC_SMOOTHING_OFF);
|
||||
}
|
||||
|
||||
// send rc smoothing details to blackbox
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
rcSmoothingFilter_t *getRcSmoothingData(void)
|
||||
{
|
||||
|
|
@ -886,6 +761,6 @@ rcSmoothingFilter_t *getRcSmoothingData(void)
|
|||
}
|
||||
|
||||
bool rcSmoothingInitializationComplete(void) {
|
||||
return (rxConfig()->rc_smoothing_type != RC_SMOOTHING_TYPE_FILTER) || rcSmoothingData.filterInitialized;
|
||||
return rcSmoothingData.filterInitialized;
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue