Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Use lrintf to round floats to integers

Browse source
This commit is contained in:
Mathias Rasmussen 2021-12-04 20:29:42 +01:00 • committed by KarateBrot
parent b84b8c1ca5
commit 34ee2f67c9
8 changed files with 29 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cms/cms_menu_misc.c
View file

@ -88,7 +88,7 @@ static const void *cmsx_menuRcOnDisplayUpdate(displayPort_t *pDisp, const OSD_En
UNUSED(selected);
for (int i = 0; i <= AUX4; i++) {
rcDataInt[i] = lroundf(rcData[i]);
rcDataInt[i] = lrintf(rcData[i]);
}
return NULL;

2
src/main/fc/rc.c
View file

@ -580,7 +580,7 @@ FAST_CODE void processRcCommand(void)
}
rawSetpoint[axis] = constrainf(angleRate, -1.0f * currentControlRateProfile->rate_limit[axis], 1.0f * currentControlRateProfile->rate_limit[axis]);
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
DEBUG_SET(DEBUG_ANGLERATE, axis, lrintf(angleRate));
}
// adjust raw setpoint steps to camera angle (mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {

4
src/main/flight/dyn_notch_filter.c
View file

@ -184,8 +184,8 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU
// the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2)
sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 18.5hz per bin at 8k and 600Hz maxHz
sdftStartBin = MAX(2, dynNotch.minHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotch.maxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
sdftStartBin = MAX(2, lrintf(dynNotch.minHz / sdftResolutionHz)); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotch.maxHz / sdftResolutionHz)); // can't use more than SDFT_BIN_COUNT bins.
pt1LooptimeS = DYN_NOTCH_CALC_TICKS / looprateHz;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {

6
src/main/flight/feedforward.c
View file

@ -197,10 +197,10 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
{
switch (axis) {
case FD_ROLL:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value);
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(value));
break;
case FD_PITCH:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value);
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(value));
break;
}
@ -213,7 +213,7 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
}
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value);
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(value));
}
return value;

26
src/main/flight/gps_rescue.c
View file

@ -333,8 +333,8 @@ static void rescueAttainPosition()
rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment;
rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, lrintf(throttleP));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, lrintf(throttleD));
/**
Heading / yaw controller
@ -410,8 +410,8 @@ static void rescueAttainPosition()
pitchAdjustment = movingAvgPitchAdjustment;
// pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100
// it gets added to the normal level mode Pitch adjustments in pid.c
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, velocityP);
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, velocityD);
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, lrintf(velocityP));
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, lrintf(velocityD));
}
const float pitchAdjustmentFiltered = pt3FilterApply(&pitchLpf, pitchAdjustment);
@ -421,8 +421,8 @@ static void rescueAttainPosition()
gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustmentFiltered, -pitchAngleLimit, pitchAngleLimit);
// this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, rescueState.intent.targetVelocityCmS);
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, rescueState.intent.targetVelocityCmS);
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, lrintf(rescueState.intent.targetVelocityCmS));
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, lrintf(rescueState.intent.targetVelocityCmS));
}
static void performSanityChecks()
@ -562,8 +562,8 @@ static void sensorUpdate()
rescueState.sensor.currentAltitudeCm = getAltitude();
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, lrintf(rescueState.sensor.currentAltitudeCm));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, lrintf(rescueState.sensor.currentAltitudeCm));
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueState.sensor.groundSpeedCmS); // groundspeed cm/s
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsSol.groundCourse); // degrees * 10
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
@ -610,8 +610,8 @@ static void sensorUpdate()
rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, rescueState.sensor.velocityToHomeCmS);
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, rescueState.sensor.velocityToHomeCmS);
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, lrintf(rescueState.sensor.velocityToHomeCmS));
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, lrintf(rescueState.sensor.velocityToHomeCmS));
}
@ -853,9 +853,9 @@ void updateGPSRescueState(void)
break;
}
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, rescueState.intent.targetAltitudeCm);
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.intent.targetAltitudeCm);
DEBUG_SET(DEBUG_RTH, 0, rescueState.intent.maxAltitudeCm);
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, lrintf(rescueState.intent.targetAltitudeCm));
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, lrintf(rescueState.intent.targetAltitudeCm));
DEBUG_SET(DEBUG_RTH, 0, lrintf(rescueState.intent.maxAltitudeCm));
performSanityChecks();
rescueAttainPosition();

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

@ -223,7 +223,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
if (mixerRuntime.dynIdleMinRps > 0.0f) {
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f;
float minRps = rpmMinMotorFrequency();
DEBUG_SET(DEBUG_DYN_IDLE, 3, (minRps * 10));
DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10));
float rpsError = mixerRuntime.dynIdleMinRps - minRps;
// PT1 type lowpass delay and smoothing for D
minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps);
@ -234,9 +234,9 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
mixerRuntime.dynIdleI += rpsError * mixerRuntime.dynIdleIGain;
mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease);
motorRangeMinIncrease = constrainf((dynIdleP + mixerRuntime.dynIdleI + dynIdleD), 0.0f, maxIncrease);
DEBUG_SET(DEBUG_DYN_IDLE, 0, (MAX(-1000.0f, dynIdleP * 10000)));
DEBUG_SET(DEBUG_DYN_IDLE, 1, (mixerRuntime.dynIdleI * 10000));
DEBUG_SET(DEBUG_DYN_IDLE, 2, (dynIdleD * 10000));
DEBUG_SET(DEBUG_DYN_IDLE, 0, MAX(-1000, lrintf(dynIdleP * 10000)));
DEBUG_SET(DEBUG_DYN_IDLE, 1, lrintf(mixerRuntime.dynIdleI * 10000));
DEBUG_SET(DEBUG_DYN_IDLE, 2, lrintf(dynIdleD * 10000));
} else {
motorRangeMinIncrease = 0;
}
@ -250,8 +250,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
// batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow
float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f);
motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor;
DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100);
DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000);
DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100));
DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000));
}
motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
#else

2
src/main/flight/rpm_filter.c
View file

@ -143,7 +143,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f);
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
filterUpdatesPerIteration = lrintf(filtersPerLoopIteration + 0.49f);
}
static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)

6
src/main/telemetry/frsky_hub.c
View file

@ -26,6 +26,7 @@
#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include "platform.h"
@ -292,10 +293,7 @@ static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum)
}
int16_t data;
if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) {
float tmp = (satellite - 32) / 1.8f;
// Round the value
tmp += (tmp < 0) ? -0.5f : 0.5f;
data = tmp;
data = lrintf((satellite - 32) / 1.8f);
} else {
data = satellite;
}