From 257865cff2f802ccaa7447cc4cf377adfda5bf25 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 3 Mar 2016 16:17:10 +0100 Subject: [PATCH] Simplify Fixed point Math notations --- src/main/common/maths.c | 13 ++++++------- src/main/common/maths.h | 26 +++++++------------------- src/main/flight/mixer.c | 14 +++++++------- src/main/sensors/battery.c | 8 ++++---- src/main/sensors/battery.h | 2 +- 5 files changed, 25 insertions(+), 38 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 8ea75ec9f..3be8eff72 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -338,15 +338,14 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count) } } -int16_t qPercent(q_number_t q) { - return 100 * q.num / q.den; +int16_t qPercent(fix12_t q) { + return (100 * q) >> 12; } -int16_t qMultiply(q_number_t q, int16_t input) { - return input * q.num / q.den; +int16_t qMultiply(fix12_t q, int16_t input) { + return (input * q) >> 12; } -void qConstruct(q_number_t *qNumber, int16_t num, int16_t den, int qType) { - qNumber->num = (1 << qType) / den; - qNumber->den = (1 << qType) / num; +fix12_t qConstruct(int16_t num, int16_t den) { + return (num << 12) / den; } diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 59a078564..4a32e282c 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -34,28 +34,16 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define ABS(x) ((x) > 0 ? (x) : -(x)) +#define Q12 (1 << 12) + +typedef int32_t fix12_t; + typedef struct stdev_s { float m_oldM, m_newM, m_oldS, m_newS; 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; @@ -125,6 +113,6 @@ float acos_approx(float x); 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); +int16_t qPercent(fix12_t q); +int16_t qMultiply(fix12_t q, int16_t input); +fix12_t qConstruct(int16_t num, int16_t den); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9935ce7ad..e0e247199 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -753,7 +753,7 @@ void acroPlusApply(void) { for (axis = 0; axis < 2; axis++) { int16_t factor; - q_number_t wowFactor; + fix12_t wowFactor; int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100) int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5; int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle; @@ -766,11 +766,11 @@ void acroPlusApply(void) { } else { rcCommandDeflection += acroPlusStickOffset; } - qConstruct(&wowFactor,ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500, Q12_NUMBER); + wowFactor = qConstruct(ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500); factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500); - wowFactor.num = wowFactor.den - wowFactor.num; + wowFactor = Q12 - wowFactor; } else { - qConstruct(&wowFactor, 1, 1, Q12_NUMBER); + wowFactor = Q12; factor = 0; } axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]); @@ -780,8 +780,8 @@ void acroPlusApply(void) { void mixTable(void) { uint32_t i; - q_number_t vbatCompensationFactor; - static q_number_t mixReduction; + fix12_t vbatCompensationFactor; + static fix12_t mixReduction; uint8_t axis; bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code @@ -857,7 +857,7 @@ void mixTable(void) if (rollPitchYawMixRange > throttleRange) { motorLimitReached = true; - qConstruct(&mixReduction, throttleRange, rollPitchYawMixRange, Q12_NUMBER); + mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 390b293f3..dcdc2167b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -215,13 +215,13 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea mAhDrawn = mAhdrawnRaw / (3600 * 100); } -q_number_t calculateVbatPidCompensation(void) { - q_number_t batteryScaler; +fix12_t calculateVbatPidCompensation(void) { + fix12_t batteryScaler; if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) { uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount; - qConstruct(&batteryScaler, maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage), Q12_NUMBER); + batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage)); } else { - qConstruct(&batteryScaler, 1, 1, Q12_NUMBER); + batteryScaler = Q12; } return batteryScaler; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 7c82078e0..f4327c065 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -77,6 +77,6 @@ batteryConfig_t *batteryConfig; void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle); int32_t currentMeterToCentiamps(uint16_t src); -q_number_t calculateVbatPidCompensation(void); +fix12_t calculateVbatPidCompensation(void); uint8_t calculateBatteryPercentage(void); uint8_t calculateBatteryCapacityRemainingPercentage(void);