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

AIR MODE implementation

Browse source
This commit is contained in:
borisbstyle 2015-12-06 02:19:35 +01:00
parent b3f0bd1402
commit 83d186057f
5 changed files with 88 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -69,6 +69,8 @@ static rxConfig_t *rxConfig;
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
float rpy_limiting = 1.0f;
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
@ -749,41 +751,88 @@ void mixTable(void)
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
}
} else {
int16_t rpy[MAX_SUPPORTED_MOTORS];
int16_t rpy_max = 0; // assumption: symetrical about zero.
int16_t rpy_min = 0;
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
rpy[i] =
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (rpy[i] > rpy_max) rpy_max = rpy[i];
if (rpy[i] < rpy_min) rpy_min = rpy[i];
}
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rpy_range = rpy_max - rpy_min;
int16_t th_range = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
int16_t th_min, th_max;
if (rpy_range > th_range) {
for (i = 0; i < motorCount; i++) {
rpy[i] = (rpy[i] * th_range) / rpy_range;
}
th_min = th_max = escAndServoConfig->minthrottle + (th_range / 2);
} else {
th_min = escAndServoConfig->minthrottle + (rpy_range / 2);
th_max = escAndServoConfig->maxthrottle - (rpy_range / 2);
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
//
// TODO: handle the case when motors don't all get the same throttle factor...
// too lazy to sort out the math right now.
for (i = 0; i < motorCount; i++) {
motor[i] = rpy[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, th_min, th_max);
}
// adjust feedback to scale PID error inputs to our limitations.
rpy_limiting = constrainf(((rpy_limiting * th_range) / rpy_range), 0.1f, 1.0f);
}
if (ARMING_FLAG(ARMED)) {
bool isFailsafeActive = failsafeIsActive();
int16_t maxThrottleDifference = 0;
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
}
}
if (maxMotor > escAndServoConfig->maxthrottle) {
maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle;
}
}
int16_t maxThrottleDifference = 0;
if (maxMotor > escAndServoConfig->maxthrottle) {
maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle;
}
for (i = 0; i < motorCount; i++) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
}
if (feature(FEATURE_3D)) {
if ((IS_RC_MODE_ACTIVE(BOXIDLE_UP))
if ((IS_RC_MODE_ACTIVE(BOXAIRMODE))
|| rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle
|| rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) {
if (rcData[THROTTLE] > rxConfig->midrc) {
@ -805,7 +854,7 @@ void mixTable(void)
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
// do not spin the motors.
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if (((rcData[THROTTLE]) < rxConfig->mincheck) && !(IS_RC_MODE_ACTIVE(BOXIDLE_UP))) {
if (((rcData[THROTTLE]) < rxConfig->mincheck) && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = escAndServoConfig->mincommand;
} else {

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

@ -50,6 +50,7 @@
extern uint16_t cycleTime;
extern uint8_t motorCount;
extern float dT;
extern float rpy_limiting;
int16_t axisPID[3];
@ -170,6 +171,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
RateError = RateError * rpy_limiting;
}
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
@ -286,6 +291,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - (gyroADC[axis] / 4);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
RateError = RateError * rpy_limiting;
}
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;

2
src/main/io/rc_controls.h
View file

@ -48,7 +48,7 @@ typedef enum {
BOXSERVO3,
BOXBLACKBOX,
BOXFAILSAFE,
BOXIDLE_UP,
BOXAIRMODE,
CHECKBOX_ITEM_COUNT
} boxId_e;

6
src/main/io/serial_msp.c
View file

@ -356,7 +356,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXIDLE_UP, "IDLE UP;", 28 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -642,7 +642,7 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIdCount = 0;
activeBoxIds[activeBoxIdCount++] = BOXARM;
activeBoxIds[activeBoxIdCount++] = BOXIDLE_UP;
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
if (sensors(SENSOR_ACC)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
@ -837,7 +837,7 @@ static bool processOutCommand(uint8_t cmdMSP)
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXIDLE_UP)) << BOXIDLE_UP;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));
if (flag)

2
src/main/mw.c
View file

@ -565,7 +565,7 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
if (throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXIDLE_UP))) {
if ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED))) {
pidResetErrorAngle();
pidResetErrorGyro();
}