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

Fixes and cleanups to the new Airmode code

Browse source
This commit is contained in:
borisbstyle 2016-02-06 22:13:44 +01:00
parent 035b34bbc4
commit 3b84cddc3d
2 changed files with 16 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

46
src/main/flight/mixer.c
View file

@ -54,12 +54,6 @@
#include "config/runtime_config.h" #include "config/runtime_config.h"
#include "config/config.h" #include "config/config.h"
typedef enum {
THROTTLE_3D_NEUTRAL = 0,
THROTTLE_3D_POSITIVE,
THROTTLE_3D_NEGATIVE
} throttle3dState_e;
uint8_t motorCount; uint8_t motorCount;
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
@ -786,42 +780,32 @@ void mixTable(void)
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle; int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax; int16_t throttleMin, throttleMax;
throttle3dState_e throttle3dPosition = THROTTLE_3D_NEUTRAL; // Initiate in Neutral before all throttle checks
throttle = rcCommand[THROTTLE];
// Find min and max throttle based on condition // Find min and max throttle based on condition
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
static int16_t throttleMinPrevious = 0, throttleMaxPrevious = 0, throttlePrevious = 0; static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
if (rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { if (rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { // Out of deadband handling
throttleMax = flight3DConfig->deadband3d_low; throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle; throttleMin = escAndServoConfig->minthrottle;
throttle3dPosition = THROTTLE_3D_NEGATIVE; throttlePrevious = throttle = rcCommand[THROTTLE];
} else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { } else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) {
throttleMax = escAndServoConfig->maxthrottle; throttleMax = escAndServoConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high; throttleMin = flight3DConfig->deadband3d_high;
throttle3dPosition = THROTTLE_3D_POSITIVE; throttlePrevious = throttle = rcCommand[THROTTLE];
} else { } else { // Deadband handling
// when starting in neutral go positive and when coming from positive. Keep positive throttle within deadband // Always start positive. When coming from positive keep positive throttle within deadband
if ((throttle3dPosition == THROTTLE_3D_NEUTRAL && !throttleMinPrevious) if ((throttlePrevious >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) || !throttlePrevious) {
|| (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious >= flight3DConfig->deadband3d_high))) {
throttleMax = escAndServoConfig->maxthrottle; throttleMax = escAndServoConfig->maxthrottle;
throttle = throttleMin = flight3DConfig->deadband3d_high; throttle = throttleMin = flight3DConfig->deadband3d_high;
// When coming from negative. Keep negative throttle within deadband // When coming from negative. Keep negative throttle within deadband
} else if (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious <= flight3DConfig->deadband3d_low)) { } else if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
throttleMax = escAndServoConfig->mincommand; throttle = throttleMax = flight3DConfig->deadband3d_low;
throttle = throttleMin = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle;
} else {
throttleMax = throttleMaxPrevious;
throttleMin = throttleMinPrevious;
throttle = throttlePrevious;
} }
} }
throttleMaxPrevious = throttleMax;
throttleMinPrevious = throttleMin;
throttlePrevious = throttle;
} else { } else {
throttle = rcCommand[THROTTLE];
throttleMin = escAndServoConfig->minthrottle; throttleMin = escAndServoConfig->minthrottle;
throttleMax = escAndServoConfig->maxthrottle; throttleMax = escAndServoConfig->maxthrottle;
} }
@ -848,12 +832,12 @@ void mixTable(void)
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
if (isFailsafeActive) { if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); mixConstrainMotorForFailsafeCondition(i);
} else if (feature(FEATURE_3D)) { } else if (feature(FEATURE_3D)) {
if (throttle3dPosition == THROTTLE_3D_NEGATIVE) { if (throttle >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
} else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
} }
} else { } else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);

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

@ -87,7 +87,7 @@ float scaleItermToRcInput(int axis) {
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0; static float antiWindUpIncrement = 0;
if (antiWindUpIncrement) antiWindUpIncrement = 0.001 * (targetLooptime / 500); // Calculate increment for 500ms period if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime; // Calculate increment for 500ms period
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */ if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */ /* Reset Iterm on high stick inputs. No scaling necessary here */