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

Separate fc_core.c from RC processing

This commit is contained in:
borisbstyle 2017-01-30 20:58:58 +01:00
parent 98f8a4d59e
commit a112c1d7d0
7 changed files with 332 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -590,6 +590,7 @@ COMMON_SRC = \
fc/fc_dispatch.c \ fc/fc_dispatch.c \
fc/fc_hardfaults.c \ fc/fc_hardfaults.c \
fc/fc_core.c \ fc/fc_core.c \
fc/fc_rc.c \
fc/fc_msp.c \ fc/fc_msp.c \
fc/fc_tasks.c \ fc/fc_tasks.c \
fc/rc_controls.c \ fc/rc_controls.c \
@ -711,7 +712,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/system.c \ drivers/system.c \
drivers/timer.c \ drivers/timer.c \
fc/fc_tasks.c \ fc/fc_tasks.c \
fc/mw.c \ fc/fc_rc.c \
fc/rc_controls.c \ fc/rc_controls.c \
fc/rc_curves.c \ fc/rc_curves.c \
fc/runtime_config.c \ fc/runtime_config.c \

241
src/main/fc/fc_core.c
View File

@ -44,6 +44,7 @@
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/cli.h" #include "fc/cli.h"
#include "fc/fc_rc.h"
#include "msp/msp_serial.h" #include "msp/msp_serial.h"
@ -90,23 +91,8 @@ uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature 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 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; bool isRXDataNew;
static bool armingCalibrationWasInitialised; 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) void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{ {
@ -129,226 +115,6 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); 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) void updateLEDs(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
@ -698,7 +464,7 @@ static void subTaskPidController(void)
uint32_t startTime; uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController() // PID - note this is function pointer set by setPIDController()
pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation); pidController(&currentProfile->pidProfile, &accelerometerConfig()->accelerometerTrims);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
} }
@ -741,8 +507,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
&& mixerConfig()->mixerMode != MIXER_FLYING_WING && mixerConfig()->mixerMode != MIXER_FLYING_WING
#endif #endif
) { ) {
rcCommand[YAW] = 0; resetYawAxis();
setpointRate[YAW] = 0;
} }
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {

4
src/main/fc/fc_core.h
View File

@ -21,6 +21,7 @@
extern int16_t magHold; extern int16_t magHold;
extern bool isRXDataNew; extern bool isRXDataNew;
extern int16_t headFreeModeHold;
union rollAndPitchTrims_u; union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
@ -34,6 +35,3 @@ void updateLEDs(void);
void updateRcCommands(void); void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs); void taskMainPidLoop(timeUs_t currentTimeUs);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);

289
src/main/fc/fc_rc.c Executable file
View File

@ -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;
}

31
src/main/fc/fc_rc.h Executable file
View File

@ -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);

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

@ -29,6 +29,8 @@
#include "common/filter.h" #include "common/filter.h"
#include "fc/fc_core.h" #include "fc/fc_core.h"
#include "fc/fc_rc.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.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. // 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) // 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]; static float previousRateError[2];
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
// Dynamic ki component to gradually scale back integration when above windup point // Dynamic ki component to gradually scale back integration when above windup point
float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);

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

@ -90,7 +90,7 @@ typedef struct pidConfig_s {
} pidConfig_t; } pidConfig_t;
union rollAndPitchTrims_u; 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 float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];