Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

make use of the US2S macro whenever possible #8331

Merged
merged 2 commits into from Sep 21, 2022
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
3 changes: 2 additions & 1 deletion src/main/common/filter.c
Expand Up @@ -27,6 +27,7 @@ FILE_COMPILE_FOR_SPEED
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/time.h"

// NULL filter
float nullFilterApply(void *filter, float input)
Expand Down Expand Up @@ -319,7 +320,7 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint

FUNCTION_COMPILE_FOR_SIZE
void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
const float dT = refreshRate * 1e-6f;
const float dT = US2S(refreshRate);

if (cutoffFrequency) {
if (filterType == FILTER_PT1) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/rc_smoothing.c
Expand Up @@ -82,7 +82,7 @@ void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs)
static int filterFrequency;
static bool initDone = false;

const float dT = getLooptime() * 1e-6f;
const float dT = US2S(getLooptime());

if (isRXDataNew) {
if (!initDone) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/gyroanalyse.c
Expand Up @@ -96,7 +96,7 @@ void gyroDataAnalyseStateInit(

for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
state->centerFrequency[axis][i] = state->maxFrequency;
pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f);
pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, US2S(filterUpdateUs));
}

}
Expand Down
8 changes: 4 additions & 4 deletions src/main/flight/pid.c
Expand Up @@ -328,11 +328,11 @@ bool pidInitFilters(void)
}

for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, refreshRate * 1e-6f);
pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
}

#ifdef USE_ANTIGRAVITY
pt1FilterInit(&antigravityThrottleLpf, pidProfile()->antigravityCutoff, TASK_PERIOD_HZ(TASK_AUX_RATE_HZ) * 1e-6f);
pt1FilterInit(&antigravityThrottleLpf, pidProfile()->antigravityCutoff, US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
#endif

#ifdef USE_D_BOOST
Expand All @@ -343,7 +343,7 @@ bool pidInitFilters(void)

if (pidProfile()->controlDerivativeLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterInit(&pidState[axis].rateTargetFilter, pt3FilterGain(pidProfile()->controlDerivativeLpfHz, refreshRate * 1e-6f));
pt3FilterInit(&pidState[axis].rateTargetFilter, pt3FilterGain(pidProfile()->controlDerivativeLpfHz, US2S(refreshRate)));
}
}

Expand Down Expand Up @@ -379,7 +379,7 @@ bool pidInitFilters(void)
void pidResetTPAFilter(void)
{
if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, TASK_PERIOD_HZ(TASK_AUX_RATE_HZ) * 1e-6f);
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/rpm_filter.c
Expand Up @@ -161,7 +161,7 @@ void rpmFiltersInit(void)
{
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++)
{
pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, RPM_FILTER_UPDATE_RATE_US * 1e-6f);
pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US));
}

rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/smith_predictor.c
Expand Up @@ -61,7 +61,7 @@ void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength
predictor->samples = (delay * 1000) / looptime;
predictor->idx = 0;
predictor->smithPredictorStrength = strength;
pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f);
pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, US2S(looptime));
} else {
predictor->enabled = false;
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/sensors/acceleration.c
Expand Up @@ -613,7 +613,7 @@ void accInitFilters(void)
accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSoftLpfFilter[axis] = &accFilter[axis].pt1;
pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime * 1e-6f);
pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, US2S(acc.accTargetLooptime));
}
break;
case FILTER_BIQUAD:
Expand All @@ -627,7 +627,7 @@ void accInitFilters(void)

}

const float accDt = acc.accTargetLooptime * 1e-6f;
const float accDt = US2S(acc.accTargetLooptime);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&accVibeFloorFilter[axis], ACC_VIBE_FLOOR_FILT_HZ, accDt);
pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
Expand Down
6 changes: 3 additions & 3 deletions src/main/sensors/battery.c
Expand Up @@ -294,7 +294,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
if (justConnected) {
pt1FilterReset(&vbatFilterState, vbat);
} else {
vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, timeDelta * 1e-6f);
vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, US2S(timeDelta));
}
}

Expand Down Expand Up @@ -534,7 +534,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
switch (batteryMetersConfig()->current.type) {
case CURRENT_SENSOR_ADC:
{
amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, US2S(timeDelta));
break;
}
case CURRENT_SENSOR_VIRTUAL:
Expand All @@ -560,7 +560,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
{
escSensorData_t * escSensor = escSensorGetData();
if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
amperage = pt1FilterApply4(&amperageFilterState, escSensor->current, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
amperage = pt1FilterApply4(&amperageFilterState, escSensor->current, AMPERAGE_LPF_FREQ, US2S(timeDelta));
}
else {
amperage = 0;
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/gyro.c
Expand Up @@ -235,7 +235,7 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
case FILTER_PT1:
*applyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = 0; axis < 3; axis++) {
pt1FilterInit(&state[axis].pt1, cutoff, looptime * 1e-6f);
pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
}
break;
case FILTER_BIQUAD:
Expand Down
4 changes: 2 additions & 2 deletions src/main/sensors/pitotmeter.c
Expand Up @@ -189,7 +189,7 @@ STATIC_PROTOTHREAD(pitotThread)

// Init filter
pitot.lastMeasurementUs = micros();
pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0);
pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);

while(1) {
// Start measurement
Expand All @@ -209,7 +209,7 @@ STATIC_PROTOTHREAD(pitotThread)

// Filter pressure
currentTimeUs = micros();
pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, (currentTimeUs - pitot.lastMeasurementUs) * 1e-6f);
pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
pitot.lastMeasurementUs = currentTimeUs;
ptDelayUs(pitot.dev.delay);

Expand Down