Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +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

21
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
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
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);