rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed point math Implementation instead of floats

This commit is contained in:
borisbstyle 2016-02-18 00:25:44 +01:00
parent 5435fd0cb7
commit 7fd88f060d
8 changed files with 82 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/main/common/maths.c
View File

@ -337,3 +337,16 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
dest[i] = array1[i] - array2[i];
}
}
int16_t qPercent(q_number_t q) {
return 100 * q.num / q.den;
}
int16_t qMultiply(q_number_t q, int16_t input) {
return input * q.num / q.den;
}
void qConstruct(q_number_t *qNumber, int16_t num, int16_t den, int qType) {
qNumber->num = (1 << qType) / den;
qNumber->den = (1 << qType) / num;
}

20
src/main/common/maths.h
View File

@ -40,6 +40,22 @@ typedef struct stdev_s
int m_n;
} stdev_t;
#define Q12(x) ((1 << 12) / x)
#define Q13(x) ((1 << 13) / x)
#define Q14(x) ((1 << 14) / x)
typedef enum {
Q12_NUMBER = 12,
Q13_NUMBER,
Q14_NUMBER
} qTypeIndex_e;
typedef struct q_number_s {
int16_t num;
int16_t den;
} q_number_t;
// Floating point 3 vector.
typedef struct fp_vector {
float X;
@ -108,3 +124,7 @@ float acos_approx(float x);
#endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
int16_t qPercent(q_number_t q);
int16_t qMultiply(q_number_t q, int16_t input);
void qConstruct(q_number_t *qNumber, int16_t num, int16_t den, int qType);

12
src/main/flight/mixer.c
View File

@ -752,7 +752,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
void mixTable(void)
{
uint32_t i;
float vbatCompensationFactor;
q_number_t vbatCompensationFactor;
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
@ -775,7 +775,7 @@ void mixTable(void)
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] *= vbatCompensationFactor; // Add voltage compensation
if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
@ -816,12 +816,14 @@ void mixTable(void)
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
float mixReduction = (float) throttleRange / rollPitchYawMixRange;
q_number_t mixReduction;
qConstruct(&mixReduction, throttleRange, rollPitchYawMixRange, Q12_NUMBER);
//float mixReduction = (float) throttleRange / rollPitchYawMixRange;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = lrintf((float) rollPitchYawMix[i] * mixReduction);
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center. Only active below 50% of saturation levels to reduce spazzing out in crashes
if ((mixReduction > (mixerConfig->airmode_saturation_limit / 100.0f)) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if ((qPercent(mixReduction) > mixerConfig->airmode_saturation_limit) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
}

53
src/main/flight/pid.c
View File

@ -94,38 +94,45 @@ void pidResetErrorGyro(void)
}
}
float scaleItermToRcInput(int axis) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0;
q_number_t scaleItermToRcInput(int axis) {
int16_t rcCommandReflection = ABS(rcCommand[axis]);
static int16_t antiWindUpIncrement;
static q_number_t qItermScaler[3] = {
{ .num= Q13(1), .den = Q13(1) },
{ .num= Q13(1), .den = Q13(1) },
{ .num= Q13(1), .den = Q13(1) },
};
if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime; // Calculate increment for 500ms period
if (!antiWindUpIncrement) {
antiWindUpIncrement = constrain(targetLooptime >> 6, 1, Q13(1)); // Calculate increment for 512ms period. Should be consistent up to 8khz
}
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
if (rcCommandReflection > 350) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
qItermScaler[axis].num = 0;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 512ms */
if (qItermScaler[axis].num <= (Q13(1) - antiWindUpIncrement)) {
qItermScaler[axis].num = qItermScaler[axis].num + antiWindUpIncrement;
} else {
iTermScaler[axis] = 1;
qItermScaler[axis].num = Q13(1);
}
}
return iTermScaler[axis];
return qItermScaler[axis];
}
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
int16_t rcCommandDeflection = constrain(rcCommand[axis], 0, 500); // Limit stick input to 500 (rcCommand 100)
qConstruct(&axisState->wowFactor, 1, 1, Q12_NUMBER);
axisState->factor = 0;
/* acro plus factor handling */
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor && (!flightModeFlags)) {
axisState->wowFactor = ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f); //0-1f
axisState->factor = axisState->wowFactor * rcCommandReflection * 1000;
axisState->wowFactor = 1.0f - axisState->wowFactor;
qConstruct(&axisState->wowFactor,ABS(rcCommandDeflection) * pidProfile->airModeInsaneAcrobilityFactor / 100, 500, Q12_NUMBER);
axisState->factor = qMultiply(axisState->wowFactor, rcCommandDeflection << 1); // Max factor 1000 on rcCommand of 500
axisState->wowFactor.num = axisState->wowFactor.den - axisState->wowFactor.num;
}
}
@ -214,7 +221,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroIf[axis] *= scaleItermToRcInput(axis);
errorGyroIf[axis] = qMultiply(scaleItermToRcInput(axis),(int16_t) errorGyroIf[axis] * 10) / 10.0f; // Scale up and down to avoid truncating
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
@ -257,7 +264,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis];
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE
@ -310,7 +317,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
errorGyroI[axis] = qMultiply(scaleItermToRcInput(axis),errorGyroI[axis]);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
@ -373,7 +380,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]);
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE
@ -510,7 +517,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
errorGyroI[axis] = qMultiply(scaleItermToRcInput(axis),errorGyroI[axis]);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
@ -551,7 +558,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]);
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE

4
src/main/flight/pid.h
View File

@ -80,8 +80,8 @@ typedef struct pidProfile_s {
} pidProfile_t;
typedef struct acroPlus_s {
float factor;
float wowFactor;
int16_t factor;
q_number_t wowFactor;
} acroPlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT];

4
src/main/mw.c
View File

@ -646,8 +646,8 @@ void taskMainPidLoop(void)
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 0.5f, dT);
debug[0] = cycleTime;
debug[1] = cycleTime - filteredCycleTime;
//debug[0] = cycleTime;
//debug[1] = cycleTime - filteredCycleTime;
imuUpdateGyroAndAttitude();

10
src/main/sensors/battery.c
View File

@ -210,11 +210,13 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
mAhDrawn = mAhdrawnRaw / (3600 * 100);
}
float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
q_number_t calculateVbatPidCompensation(void) {
q_number_t batteryScaler;
if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) {
// Up to 25% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.25f);
uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount;
qConstruct(&batteryScaler, maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage), Q12_NUMBER);
} else {
qConstruct(&batteryScaler, 1, 1, Q12_NUMBER);
}
return batteryScaler;

3
src/main/sensors/battery.h
View File

@ -18,6 +18,7 @@
#pragma once
#include "rx/rx.h"
#include "common/maths.h"
#define VBAT_SCALE_DEFAULT 110
#define VBAT_RESDIVVAL_DEFAULT 10
@ -76,6 +77,6 @@ batteryConfig_t *batteryConfig;
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);
q_number_t calculateVbatPidCompensation(void);
uint8_t calculateBatteryPercentage(void);
uint8_t calculateBatteryCapacityRemainingPercentage(void);