diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 3754a4e616..2511fcfd5c 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -173,11 +173,11 @@ static void scaleRcCommandToFpvCamAngle(void) { void processRcCommand(void) { - static int16_t lastCommand[4] = { 0, 0, 0, 0 }; - static int16_t deltaRC[4] = { 0, 0, 0, 0 }; - static int16_t factor, rcInterpolationFactor; + static float rcCommandInterp[4] = { 0, 0, 0, 0 }; + static float rcStepSize[4] = { 0, 0, 0, 0 }; + static int16_t rcInterpolationStepCount; static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" uint16_t rxRefreshRate; bool readyToCalculateRate = false; uint8_t readyToCalculateRateAxisCnt = 0; @@ -204,36 +204,34 @@ void processRcCommand(void) rxRefreshRate = rxGetRefreshRate(); } - if (isRXDataNew) { - rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; - - if (debugMode == DEBUG_RC_INTERPOLATION) { - for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; - debug[3] = rxRefreshRate; - } + if (isRXDataNew && rxRefreshRate > 0) { + rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; for (int channel=ROLL; channel < interpolationChannels; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; + rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount; } - factor = rcInterpolationFactor - 1; + if (debugMode == DEBUG_RC_INTERPOLATION) { + debug[0] = lrintf(rcCommand[0]); + debug[1] = lrintf(getTaskDeltaTime(TASK_RX) / 1000); + //debug[1] = lrintf(rcCommandInterp[0]); + //debug[1] = lrintf(rcStepSize[0]*100); + } } else { - factor--; + rcInterpolationStepCount--; } // Interpolate steps of rcCommand - if (factor > 0) { + if (rcInterpolationStepCount > 0) { for (int channel=ROLL; channel < interpolationChannels; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation - readyToCalculateRate = true; + rcCommandInterp[channel] += rcStepSize[channel]; + rcCommand[channel] = rcCommandInterp[channel]; + readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation } - } else { - factor = 0; + readyToCalculateRate = true; } } else { - factor = 0; // reset factor in case of level modes flip flopping + rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping } if (readyToCalculateRate || isRXDataNew) { @@ -243,6 +241,10 @@ void processRcCommand(void) for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) calculateSetpointRate(axis); + if (debugMode == DEBUG_RC_INTERPOLATION) { + debug[2] = rcInterpolationStepCount; + debug[3] = setpointRate[0]; + } // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); @@ -311,7 +313,7 @@ void updateRcCommands(void) const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); - const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; + const float rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 06208befd2..bdc5e2ae94 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -65,7 +65,7 @@ static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) static bool isUsingSticksToArm = true; -int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW +float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index fe35b88e08..9be4198d2a 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -151,7 +151,7 @@ typedef struct modeActivationProfile_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -extern int16_t rcCommand[4]; +extern float rcCommand[4]; typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 08f750e7c7..7fbb9838b8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -421,13 +421,18 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate D component if (axis != FD_YAW) { + // apply filters + float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); + gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); + float dynC = dtermSetpointWeight; if (pidProfile->setpointRelaxRatio < 100) { dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } - const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y + const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y // Divide rate change by dT to get differential (ie dr/dt) float delta = (rD - previousRateError[axis]) / dT; + previousRateError[axis] = rD; // if crash recovery is on check for a crash @@ -441,10 +446,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } - // apply filters - delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta); - delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta); - axisPID_D[axis] = Kd[axis] * delta * tpaFactor; } diff --git a/src/test/unit/altitude_hold_unittest.cc.txt b/src/test/unit/altitude_hold_unittest.cc.txt index d3bd1c8560..863b6b9cdd 100644 --- a/src/test/unit/altitude_hold_unittest.cc.txt +++ b/src/test/unit/altitude_hold_unittest.cc.txt @@ -133,7 +133,7 @@ TEST(AltitudeHoldTest, TestCalculateTiltAngle) extern "C" { uint32_t rcModeActivationMask; -int16_t rcCommand[4]; +float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t accTimeSum ; // keep track for integration of acc diff --git a/src/test/unit/battery_unittest.cc.txt b/src/test/unit/battery_unittest.cc.txt index 3834ff7540..d3a8cb889d 100644 --- a/src/test/unit/battery_unittest.cc.txt +++ b/src/test/unit/battery_unittest.cc.txt @@ -270,7 +270,7 @@ TEST(BatteryTest, RollOverPattern2) extern "C" { uint8_t armingFlags = 0; -int16_t rcCommand[4] = {0,0,0,0}; +float rcCommand[4] = {0,0,0,0}; bool feature(uint32_t mask) diff --git a/src/test/unit/flight_failsafe_unittest.cc.txt b/src/test/unit/flight_failsafe_unittest.cc.txt index 581f06a89a..c82c424b27 100644 --- a/src/test/unit/flight_failsafe_unittest.cc.txt +++ b/src/test/unit/flight_failsafe_unittest.cc.txt @@ -406,7 +406,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; -int16_t rcCommand[4]; +float rcCommand[4]; uint32_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; bool isUsingSticksToArm = true; diff --git a/src/test/unit/flight_imu_unittest.cc.txt b/src/test/unit/flight_imu_unittest.cc.txt index c87e3d54b0..797c539b3f 100644 --- a/src/test/unit/flight_imu_unittest.cc.txt +++ b/src/test/unit/flight_imu_unittest.cc.txt @@ -76,7 +76,7 @@ TEST(FlightImuTest, TestCalculateHeading) extern "C" { uint32_t rcModeActivationMask; -int16_t rcCommand[4]; +float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; acc_t acc; diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 9178976d8e..8a30af72fd 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -380,7 +380,7 @@ rollAndPitchInclination_t inclination; rxRuntimeConfig_t rxRuntimeConfig; int16_t axisPID[XYZ_AXIS_COUNT]; -int16_t rcCommand[4]; +float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t rcModeActivationMask; diff --git a/src/test/unit/ledstrip_unittest.cc.txt b/src/test/unit/ledstrip_unittest.cc.txt index 4e518163c3..2f53aad885 100644 --- a/src/test/unit/ledstrip_unittest.cc.txt +++ b/src/test/unit/ledstrip_unittest.cc.txt @@ -351,7 +351,7 @@ extern "C" { uint8_t armingFlags = 0; uint16_t flightModeFlags = 0; -int16_t rcCommand[4]; +float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t rcModeActivationMask; diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index c9a249968d..4537a82180 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -38,7 +38,7 @@ extern "C" { extern "C" { uint8_t batteryCellCount = 3; - int16_t rcCommand[4] = {0, 0, 0, 0}; + float rcCommand[4] = {0, 0, 0, 0}; int16_t telemTemperature1 = 0; baro_t baro = { .baroTemperature = 50 }; telemetryConfig_t telemetryConfig_System; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index 2dd03a924b..bdbe1b0199 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -38,7 +38,7 @@ extern "C" { extern "C" { uint8_t testBatteryCellCount =3; - int16_t rcCommand[4] = {0, 0, 0, 0}; + float rcCommand[4] = {0, 0, 0, 0}; telemetryConfig_t telemetryConfig_System; }