Separate fc_core.c from RC processing
This commit is contained in:
parent
98f8a4d59e
commit
3d8c098bd8
3
Makefile
3
Makefile
|
@ -590,6 +590,7 @@ COMMON_SRC = \
|
|||
fc/fc_dispatch.c \
|
||||
fc/fc_hardfaults.c \
|
||||
fc/fc_core.c \
|
||||
fc/fc_rc.c \
|
||||
fc/fc_msp.c \
|
||||
fc/fc_tasks.c \
|
||||
fc/rc_controls.c \
|
||||
|
@ -711,7 +712,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
drivers/system.c \
|
||||
drivers/timer.c \
|
||||
fc/fc_tasks.c \
|
||||
fc/mw.c \
|
||||
fc/fc_rc.c \
|
||||
fc/rc_controls.c \
|
||||
fc/rc_curves.c \
|
||||
fc/runtime_config.c \
|
||||
|
|
|
@ -44,6 +44,7 @@
|
|||
#include "fc/rc_curves.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/cli.h"
|
||||
#include "fc/fc_rc.h"
|
||||
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
|
@ -90,23 +91,8 @@ uint8_t motorControlEnable = false;
|
|||
int16_t telemTemperature1; // gyro sensor temperature
|
||||
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
||||
|
||||
static float throttlePIDAttenuation;
|
||||
|
||||
bool isRXDataNew;
|
||||
static bool armingCalibrationWasInitialised;
|
||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||
|
||||
float getSetpointRate(int axis) {
|
||||
return setpointRate[axis];
|
||||
}
|
||||
|
||||
float getRcDeflection(int axis) {
|
||||
return rcDeflection[axis];
|
||||
}
|
||||
|
||||
float getRcDeflectionAbs(int axis) {
|
||||
return rcDeflectionAbs[axis];
|
||||
}
|
||||
|
||||
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
|
||||
{
|
||||
|
@ -129,226 +115,6 @@ bool isCalibrating()
|
|||
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
|
||||
}
|
||||
|
||||
#define SETPOINT_RATE_LIMIT 1998.0f
|
||||
#define RC_RATE_INCREMENTAL 14.54f
|
||||
|
||||
void calculateSetpointRate(int axis, int16_t rc) {
|
||||
float angleRate, rcRate, rcSuperfactor, rcCommandf;
|
||||
uint8_t rcExpo;
|
||||
|
||||
if (axis != YAW) {
|
||||
rcExpo = currentControlRateProfile->rcExpo8;
|
||||
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
|
||||
} else {
|
||||
rcExpo = currentControlRateProfile->rcYawExpo8;
|
||||
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
|
||||
}
|
||||
|
||||
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
|
||||
rcCommandf = rc / 500.0f;
|
||||
rcDeflection[axis] = rcCommandf;
|
||||
rcDeflectionAbs[axis] = ABS(rcCommandf);
|
||||
|
||||
if (rcExpo) {
|
||||
float expof = rcExpo / 100.0f;
|
||||
rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof);
|
||||
}
|
||||
|
||||
angleRate = 200.0f * rcRate * rcCommandf;
|
||||
|
||||
if (currentControlRateProfile->rates[axis]) {
|
||||
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
|
||||
angleRate *= rcSuperfactor;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
|
||||
|
||||
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
|
||||
}
|
||||
|
||||
void scaleRcCommandToFpvCamAngle(void) {
|
||||
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
|
||||
static uint8_t lastFpvCamAngleDegrees = 0;
|
||||
static float cosFactor = 1.0;
|
||||
static float sinFactor = 0.0;
|
||||
|
||||
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
|
||||
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
|
||||
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
|
||||
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
|
||||
}
|
||||
|
||||
float roll = setpointRate[ROLL];
|
||||
float yaw = setpointRate[YAW];
|
||||
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
|
||||
setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
|
||||
}
|
||||
|
||||
#define THROTTLE_BUFFER_MAX 20
|
||||
#define THROTTLE_DELTA_MS 100
|
||||
|
||||
void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
|
||||
static int index;
|
||||
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
|
||||
const int rxRefreshRateMs = rxRefreshRate / 1000;
|
||||
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
|
||||
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
|
||||
|
||||
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
|
||||
if (index >= indexMax)
|
||||
index = 0;
|
||||
|
||||
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
|
||||
|
||||
if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
|
||||
pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain);
|
||||
else
|
||||
pidSetItermAccelerator(1.0f);
|
||||
}
|
||||
|
||||
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 uint16_t currentRxRefreshRate;
|
||||
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
|
||||
uint16_t rxRefreshRate;
|
||||
bool readyToCalculateRate = false;
|
||||
uint8_t readyToCalculateRateAxisCnt = 0;
|
||||
|
||||
if (isRXDataNew) {
|
||||
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
|
||||
checkForThrottleErrorResetState(currentRxRefreshRate);
|
||||
}
|
||||
|
||||
if (rxConfig()->rcInterpolation || flightModeFlags) {
|
||||
// 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) {
|
||||
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
|
||||
|
||||
if (debugMode == DEBUG_RC_INTERPOLATION) {
|
||||
for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
|
||||
debug[3] = rxRefreshRate;
|
||||
}
|
||||
|
||||
for (int channel=ROLL; channel < interpolationChannels; channel++) {
|
||||
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
|
||||
lastCommand[channel] = rcCommand[channel];
|
||||
}
|
||||
|
||||
factor = rcInterpolationFactor - 1;
|
||||
} else {
|
||||
factor--;
|
||||
}
|
||||
|
||||
// Interpolate steps of rcCommand
|
||||
if (factor > 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;
|
||||
}
|
||||
} else {
|
||||
factor = 0;
|
||||
}
|
||||
} else {
|
||||
factor = 0; // reset factor in case of level modes flip flopping
|
||||
}
|
||||
|
||||
if (readyToCalculateRate || isRXDataNew) {
|
||||
if (isRXDataNew)
|
||||
readyToCalculateRateAxisCnt = FD_YAW;
|
||||
|
||||
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
|
||||
calculateSetpointRate(axis, rcCommand[axis]);
|
||||
|
||||
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
|
||||
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
|
||||
scaleRcCommandToFpvCamAngle();
|
||||
|
||||
isRXDataNew = false;
|
||||
}
|
||||
}
|
||||
|
||||
void updateRcCommands(void)
|
||||
{
|
||||
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
|
||||
int32_t prop;
|
||||
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
|
||||
prop = 100;
|
||||
throttlePIDAttenuation = 1.0f;
|
||||
} else {
|
||||
if (rcData[THROTTLE] < 2000) {
|
||||
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
|
||||
} else {
|
||||
prop = 100 - currentControlRateProfile->dynThrPID;
|
||||
}
|
||||
throttlePIDAttenuation = prop / 100.0f;
|
||||
}
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
||||
|
||||
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||
if (axis == ROLL || axis == PITCH) {
|
||||
if (tmp > rcControlsConfig()->deadband) {
|
||||
tmp -= rcControlsConfig()->deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
rcCommand[axis] = tmp;
|
||||
} else {
|
||||
if (tmp > rcControlsConfig()->yaw_deadband) {
|
||||
tmp -= rcControlsConfig()->yaw_deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
|
||||
}
|
||||
if (rcData[axis] < rxConfig()->midrc) {
|
||||
rcCommand[axis] = -rcCommand[axis];
|
||||
}
|
||||
}
|
||||
|
||||
int32_t tmp;
|
||||
if (feature(FEATURE_3D)) {
|
||||
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
|
||||
} else {
|
||||
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
|
||||
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
|
||||
}
|
||||
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
|
||||
|
||||
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
|
||||
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
|
||||
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
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;
|
||||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
||||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
}
|
||||
}
|
||||
|
||||
void updateLEDs(void)
|
||||
{
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
|
@ -698,7 +464,7 @@ static void subTaskPidController(void)
|
|||
uint32_t startTime;
|
||||
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation);
|
||||
pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims);
|
||||
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
|
||||
}
|
||||
|
||||
|
@ -741,8 +507,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
|||
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
||||
#endif
|
||||
) {
|
||||
rcCommand[YAW] = 0;
|
||||
setpointRate[YAW] = 0;
|
||||
resetYawAxis();
|
||||
}
|
||||
|
||||
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
extern int16_t magHold;
|
||||
extern bool isRXDataNew;
|
||||
extern int16_t headFreeModeHold;
|
||||
|
||||
union rollAndPitchTrims_u;
|
||||
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
|
||||
|
@ -34,6 +35,3 @@ void updateLEDs(void);
|
|||
void updateRcCommands(void);
|
||||
|
||||
void taskMainPidLoop(timeUs_t currentTimeUs);
|
||||
float getSetpointRate(int axis);
|
||||
float getRcDeflection(int axis);
|
||||
float getRcDeflectionAbs(int axis);
|
||||
|
|
|
@ -0,0 +1,289 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_curves.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/fc_rc.h"
|
||||
#include "fc/fc_core.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||
static float throttlePIDAttenuation;
|
||||
|
||||
float getSetpointRate(int axis) {
|
||||
return setpointRate[axis];
|
||||
}
|
||||
|
||||
float getRcDeflection(int axis) {
|
||||
return rcDeflection[axis];
|
||||
}
|
||||
|
||||
float getRcDeflectionAbs(int axis) {
|
||||
return rcDeflectionAbs[axis];
|
||||
}
|
||||
|
||||
float getThrottlePIDAttenuation(void) {
|
||||
return throttlePIDAttenuation;
|
||||
}
|
||||
|
||||
#define SETPOINT_RATE_LIMIT 1998.0f
|
||||
#define RC_RATE_INCREMENTAL 14.54f
|
||||
|
||||
void calculateSetpointRate(int axis, int16_t rc) {
|
||||
float angleRate, rcRate, rcSuperfactor, rcCommandf;
|
||||
uint8_t rcExpo;
|
||||
|
||||
if (axis != YAW) {
|
||||
rcExpo = currentControlRateProfile->rcExpo8;
|
||||
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
|
||||
} else {
|
||||
rcExpo = currentControlRateProfile->rcYawExpo8;
|
||||
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
|
||||
}
|
||||
|
||||
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
|
||||
rcCommandf = rc / 500.0f;
|
||||
rcDeflection[axis] = rcCommandf;
|
||||
rcDeflectionAbs[axis] = ABS(rcCommandf);
|
||||
|
||||
if (rcExpo) {
|
||||
float expof = rcExpo / 100.0f;
|
||||
rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof);
|
||||
}
|
||||
|
||||
angleRate = 200.0f * rcRate * rcCommandf;
|
||||
|
||||
if (currentControlRateProfile->rates[axis]) {
|
||||
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
|
||||
angleRate *= rcSuperfactor;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
|
||||
|
||||
setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
|
||||
}
|
||||
|
||||
void scaleRcCommandToFpvCamAngle(void) {
|
||||
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
|
||||
static uint8_t lastFpvCamAngleDegrees = 0;
|
||||
static float cosFactor = 1.0;
|
||||
static float sinFactor = 0.0;
|
||||
|
||||
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
|
||||
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
|
||||
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
|
||||
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
|
||||
}
|
||||
|
||||
float roll = setpointRate[ROLL];
|
||||
float yaw = setpointRate[YAW];
|
||||
setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
|
||||
setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT);
|
||||
}
|
||||
|
||||
#define THROTTLE_BUFFER_MAX 20
|
||||
#define THROTTLE_DELTA_MS 100
|
||||
|
||||
void checkForThrottleErrorResetState(uint16_t rxRefreshRate) {
|
||||
static int index;
|
||||
static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX];
|
||||
const int rxRefreshRateMs = rxRefreshRate / 1000;
|
||||
const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX);
|
||||
const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold;
|
||||
|
||||
rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE];
|
||||
if (index >= indexMax)
|
||||
index = 0;
|
||||
|
||||
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
|
||||
|
||||
if(ABS(rcCommandSpeed) > throttleVelocityThreshold)
|
||||
pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain);
|
||||
else
|
||||
pidSetItermAccelerator(1.0f);
|
||||
}
|
||||
|
||||
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 uint16_t currentRxRefreshRate;
|
||||
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
|
||||
uint16_t rxRefreshRate;
|
||||
bool readyToCalculateRate = false;
|
||||
uint8_t readyToCalculateRateAxisCnt = 0;
|
||||
|
||||
if (isRXDataNew) {
|
||||
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
|
||||
checkForThrottleErrorResetState(currentRxRefreshRate);
|
||||
}
|
||||
|
||||
if (rxConfig()->rcInterpolation || flightModeFlags) {
|
||||
// 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) {
|
||||
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
|
||||
|
||||
if (debugMode == DEBUG_RC_INTERPOLATION) {
|
||||
for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
|
||||
debug[3] = rxRefreshRate;
|
||||
}
|
||||
|
||||
for (int channel=ROLL; channel < interpolationChannels; channel++) {
|
||||
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
|
||||
lastCommand[channel] = rcCommand[channel];
|
||||
}
|
||||
|
||||
factor = rcInterpolationFactor - 1;
|
||||
} else {
|
||||
factor--;
|
||||
}
|
||||
|
||||
// Interpolate steps of rcCommand
|
||||
if (factor > 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;
|
||||
}
|
||||
} else {
|
||||
factor = 0;
|
||||
}
|
||||
} else {
|
||||
factor = 0; // reset factor in case of level modes flip flopping
|
||||
}
|
||||
|
||||
if (readyToCalculateRate || isRXDataNew) {
|
||||
if (isRXDataNew)
|
||||
readyToCalculateRateAxisCnt = FD_YAW;
|
||||
|
||||
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
|
||||
calculateSetpointRate(axis, rcCommand[axis]);
|
||||
|
||||
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
|
||||
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
|
||||
scaleRcCommandToFpvCamAngle();
|
||||
|
||||
isRXDataNew = false;
|
||||
}
|
||||
}
|
||||
|
||||
void updateRcCommands(void)
|
||||
{
|
||||
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
|
||||
int32_t prop;
|
||||
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
|
||||
prop = 100;
|
||||
throttlePIDAttenuation = 1.0f;
|
||||
} else {
|
||||
if (rcData[THROTTLE] < 2000) {
|
||||
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
|
||||
} else {
|
||||
prop = 100 - currentControlRateProfile->dynThrPID;
|
||||
}
|
||||
throttlePIDAttenuation = prop / 100.0f;
|
||||
}
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
||||
|
||||
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||
if (axis == ROLL || axis == PITCH) {
|
||||
if (tmp > rcControlsConfig()->deadband) {
|
||||
tmp -= rcControlsConfig()->deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
rcCommand[axis] = tmp;
|
||||
} else {
|
||||
if (tmp > rcControlsConfig()->yaw_deadband) {
|
||||
tmp -= rcControlsConfig()->yaw_deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
|
||||
}
|
||||
if (rcData[axis] < rxConfig()->midrc) {
|
||||
rcCommand[axis] = -rcCommand[axis];
|
||||
}
|
||||
}
|
||||
|
||||
int32_t tmp;
|
||||
if (feature(FEATURE_3D)) {
|
||||
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
|
||||
} else {
|
||||
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
|
||||
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
|
||||
}
|
||||
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
|
||||
|
||||
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
|
||||
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
|
||||
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
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;
|
||||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
||||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
}
|
||||
}
|
||||
|
||||
void resetYawAxis(void) {
|
||||
rcCommand[YAW] = 0;
|
||||
setpointRate[YAW] = 0;
|
||||
}
|
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
void calculateSetpointRate(int axis, int16_t rc);
|
||||
void scaleRcCommandToFpvCamAngle(void);
|
||||
void checkForThrottleErrorResetState(uint16_t rxRefreshRate);
|
||||
void checkForThrottleErrorResetState(uint16_t rxRefreshRate);
|
||||
void processRcCommand(void);
|
||||
float getSetpointRate(int axis);
|
||||
float getRcDeflection(int axis);
|
||||
float getRcDeflectionAbs(int axis);
|
||||
float getThrottlePIDAttenuation(void);
|
||||
void updateRcCommands(void);
|
||||
void resetYawAxis(void);
|
|
@ -29,6 +29,8 @@
|
|||
#include "common/filter.h"
|
||||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/fc_rc.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
@ -211,11 +213,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
|
|||
|
||||
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
|
||||
// Based on 2DOF reference design (matlab)
|
||||
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor)
|
||||
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
static float previousRateError[2];
|
||||
|
||||
const float tpaFactor = getThrottlePIDAttenuation();
|
||||
const float motorMixRange = getMotorMixRange();
|
||||
|
||||
// Dynamic ki component to gradually scale back integration when above windup point
|
||||
float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
|
||||
|
||||
|
|
|
@ -90,7 +90,7 @@ typedef struct pidConfig_s {
|
|||
} pidConfig_t;
|
||||
|
||||
union rollAndPitchTrims_u;
|
||||
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor);
|
||||
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
|
||||
|
||||
extern float axisPIDf[3];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
|
|
Loading…
Reference in New Issue