diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 149244b0f7..b5decef45b 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -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; diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 7ff195d4ff..5d72b30cfa 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -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)) { diff --git a/src/main/flight/dyn_notch_filter.c b/src/main/flight/dyn_notch_filter.c index b46343b170..59b3a5da01 100644 --- a/src/main/flight/dyn_notch_filter.c +++ b/src/main/flight/dyn_notch_filter.c @@ -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++) { diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c index 0158b1b4a6..068f4991c6 100644 --- a/src/main/flight/feedforward.c +++ b/src/main/flight/feedforward.c @@ -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; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 3068cdbde2..62ce1269a6 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -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(); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d0edfc1710..016dfbf11a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 1f9eae5174..48b227e32a 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -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) diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 48d12b70dc..fdd302d23d 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -26,6 +26,7 @@ #include #include #include +#include #include #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; }