Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +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); UNUSED(selected);
for (int i = 0; i <= AUX4; i++) { for (int i = 0; i <= AUX4; i++) {
rcDataInt[i] = lroundf(rcData[i]); rcDataInt[i] = lrintf(rcData[i]);
} }
return NULL; 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]); 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) // adjust raw setpoint steps to camera angle (mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { 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) // 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 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. sdftStartBin = MAX(2, lrintf(dynNotch.minHz / sdftResolutionHz)); // 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. 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; pt1LooptimeS = DYN_NOTCH_CALC_TICKS / looprateHz;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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) { switch (axis) {
case FD_ROLL: case FD_ROLL:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value); DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(value));
break; break;
case FD_PITCH: case FD_PITCH:
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value); DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(value));
break; break;
} }
@ -213,7 +213,7 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
} }
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value); DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(value));
} }
return value; return value;

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

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

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

@ -223,7 +223,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
if (mixerRuntime.dynIdleMinRps > 0.0f) { if (mixerRuntime.dynIdleMinRps > 0.0f) {
const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f; const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f;
float minRps = rpmMinMotorFrequency(); 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; float rpsError = mixerRuntime.dynIdleMinRps - minRps;
// PT1 type lowpass delay and smoothing for D // PT1 type lowpass delay and smoothing for D
minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps); 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 += rpsError * mixerRuntime.dynIdleIGain;
mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease); mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease);
motorRangeMinIncrease = constrainf((dynIdleP + mixerRuntime.dynIdleI + dynIdleD), 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, 0, MAX(-1000, lrintf(dynIdleP * 10000)));
DEBUG_SET(DEBUG_DYN_IDLE, 1, (mixerRuntime.dynIdleI * 10000)); DEBUG_SET(DEBUG_DYN_IDLE, 1, lrintf(mixerRuntime.dynIdleI * 10000));
DEBUG_SET(DEBUG_DYN_IDLE, 2, (dynIdleD * 10000)); DEBUG_SET(DEBUG_DYN_IDLE, 2, lrintf(dynIdleD * 10000));
} else { } else {
motorRangeMinIncrease = 0; 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 // 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); float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f);
motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor; motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor;
DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100); DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100));
DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000); DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000));
} }
motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
#else #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); const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f);
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics); numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate; 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) 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 <stddef.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include "platform.h" #include "platform.h"
@ -292,10 +293,7 @@ static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum)
} }
int16_t data; int16_t data;
if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) { if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) {
float tmp = (satellite - 32) / 1.8f; data = lrintf((satellite - 32) / 1.8f);
// Round the value
tmp += (tmp < 0) ? -0.5f : 0.5f;
data = tmp;
} else { } else {
data = satellite; data = satellite;
} }