Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 08:45:36 +03:00
Code Issues Wiki Activity

Separated out pidConfig_t items

Browse source
This commit is contained in:
Martin Budden 2016-12-07 16:32:33 +00:00
parent d93224be06
commit d72b4e96c9
13 changed files with 33 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -163,8 +163,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
// 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)
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
{
static float previousRateError[2];
static float previousSetpoint[3];
@ -217,13 +216,12 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
float errorAngle = pidProfile->levelSensitivity * rcCommand[axis];
#ifdef GPS
const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else
const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
errorAngle += GPS_angle[axis];
#endif
errorAngle = constrainf(errorAngle, -max_angle_inclination, max_angle_inclination);
errorAngle = (errorAngle - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;