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

slow change althold now controls velocity directly

Browse source 
The throttle stick input now directly sets the vertical velocity
setpoint of the copter. (alt_hold_fast_change = 0)

Conflicts:

	src/imu.c
	src/mw.c
	src/mw.h
This commit is contained in:
luggi 2014-07-16 18:20:09 -04:00 • committed by Dominic Clifton
parent ff82839f89
commit 7917e44293
3 changed files with 24 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/flight/altitudehold.c
View file

@ -63,12 +63,11 @@ static int16_t initialThrottleHold;
static void multirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
// multirotor alt hold
if (currentProfile.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
errorAltitudeI = 0;
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_deadband : currentProfile.alt_hold_deadband;
} else {
@ -76,22 +75,21 @@ static void multirotorAltHold(void)
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_deadband) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
AltHold += AltHoldCorr / 2000;
AltHoldCorr %= 2000;
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHoldCorr = 0;
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
}
}
@ -121,7 +119,7 @@ void updateAltHoldState(void)
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
errorVelocityI = 0;
BaroPID = 0;
}
} else {

17
src/main/flight/imu.c
View file

@ -56,7 +56,9 @@ int16_t smallAngle = 0;
int32_t EstAlt; // in cm
int32_t AltHold;
int32_t errorAltitudeI = 0;
int32_t setVelocity = 0;
uint8_t velocityControl = 0;
int32_t errorVelocityI = 0;
int32_t vario = 0; // variometer in cm/s
@ -394,10 +396,13 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
// Altitude P-Controller
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
} else {
setVel = setVelocity;
}
// Velocity PID-Controller
// P
@ -405,9 +410,9 @@ int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
errorAltitudeI += (pidProfile->I8[PIDVEL] * error);
errorAltitudeI = constrain(errorAltitudeI, -(8192 * 200), (8192 * 200));
newBaroPID += errorAltitudeI / 8192; // I in range +/-200
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
newBaroPID += errorVelocityI / 8192; // I in range +/-200
// D
newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);

4
src/main/flight/imu.h
View file

@ -17,7 +17,9 @@
#pragma once
extern int32_t errorAltitudeI;
extern int32_t errorVelocityI;
extern uint8_t velocityControl;
extern int32_t setVelocity;
extern int32_t BaroPID;
extern int16_t throttleAngleCorrection;