Simplify Fixed point Math notations
This commit is contained in:
parent
431ca856cf
commit
257865cff2
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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]);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue