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

Quaternion-based DCM IMU (original code my S.Madgwick)

Browse source 
Restore binaries from merge
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-10-22 13:36:17 +10:00 • committed by borisbstyle
parent 846b86f489
commit 5df8ca926c
21 changed files with 379 additions and 738 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -51,7 +51,6 @@ extern uint16_t cycleTime;
extern uint8_t motorCount;
extern float dT;
int16_t heading;
int16_t axisPID[3];
#ifdef BLACKBOX
@ -144,10 +143,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// calculate error and limit the angle to the max inclination
#ifdef GPS
errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f;
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#else
errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f;
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
@ -238,10 +237,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
@ -341,10 +340,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// 50 degrees max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here
@ -447,10 +446,10 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
// observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
@ -578,9 +577,9 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
@ -730,10 +729,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// calculate error and limit the angle to max configured inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#endif
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID