mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 11:59:58 +03:00
Use std abs() instead of ABS() macro
Changes uses of ABS() macro to <stdlib.h> abs() and <math.h> fabsf()
This commit is contained in:
parent
b54174c77e
commit
61ded491cd
18 changed files with 43 additions and 30 deletions
|
@ -19,6 +19,7 @@
|
|||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -117,7 +118,7 @@ int gcd(int num, int denom)
|
|||
|
||||
int32_t applyDeadband(const int32_t value, const int32_t deadband)
|
||||
{
|
||||
if (ABS(value) < deadband) {
|
||||
if (abs(value) < deadband) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
@ -25,6 +25,7 @@
|
|||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
@ -150,7 +151,7 @@ static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offsetMinute
|
|||
// Apply offset if necessary
|
||||
if (offsetMinutes != 0) {
|
||||
tz_hours = offsetMinutes / 60;
|
||||
tz_minutes = ABS(offsetMinutes % 60);
|
||||
tz_minutes = abs(offsetMinutes % 60);
|
||||
dateTimeWithOffset(&local, dateTime, offsetMinutes);
|
||||
dateTime = &local;
|
||||
}
|
||||
|
@ -172,7 +173,7 @@ static bool dateTimeFormat(char *buf, dateTime_t *dateTime, int16_t offsetMinute
|
|||
tfp_sprintf(buf, "%04u-%02u-%02uT%02u:%02u:%02u.%03u%c%02d:%02d",
|
||||
dateTime->year, dateTime->month, dateTime->day,
|
||||
dateTime->hours, dateTime->minutes, dateTime->seconds, dateTime->millis,
|
||||
tz_hours >= 0 ? '+' : '-', ABS(tz_hours), tz_minutes);
|
||||
tz_hours >= 0 ? '+' : '-', abs(tz_hours), tz_minutes);
|
||||
}
|
||||
|
||||
return retVal;
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
|
@ -166,7 +167,7 @@ char *ftoa(float x, char *floatString)
|
|||
|
||||
value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer
|
||||
|
||||
itoa(ABS(value), intString1, 10); // Create string from abs of integer value
|
||||
itoa(abs(value), intString1, 10); // Create string from abs of integer value
|
||||
|
||||
if (value >= 0)
|
||||
intString2[0] = ' '; // Positive number, add a pad space
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
|
@ -741,7 +742,7 @@ int8_t calculateThrottlePercent(void)
|
|||
|
||||
uint8_t calculateThrottlePercentAbs(void)
|
||||
{
|
||||
return ABS(calculateThrottlePercent());
|
||||
return abs(calculateThrottlePercent());
|
||||
}
|
||||
|
||||
static bool airmodeIsActivated;
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -463,7 +464,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
|
|||
// During initial training process all samples.
|
||||
// During retraining check samples to determine if they vary by more than the limit percentage.
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
|
||||
const float percentChange = (abs(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
|
||||
if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
|
||||
// We received a sample that wasn't more than the limit percent so reset the accumulation
|
||||
// During retraining we need a contiguous block of samples that are all significantly different than the current average
|
||||
|
@ -601,7 +602,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
|
|||
for (int axis = 0; axis < 3; axis++) {
|
||||
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
||||
|
||||
float tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||
float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
|
||||
if (axis == ROLL || axis == PITCH) {
|
||||
if (tmp > rcControlsConfig()->deadband) {
|
||||
tmp -= rcControlsConfig()->deadband;
|
||||
|
|
|
@ -20,8 +20,8 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -410,9 +410,8 @@ void processRcStickPositions()
|
|||
#endif
|
||||
}
|
||||
|
||||
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc)
|
||||
{
|
||||
return MIN(ABS(rcData[axis] - midrc), 500);
|
||||
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
|
||||
return MIN(abs((int32_t)rcData[axis] - midrc), 500);
|
||||
}
|
||||
|
||||
void rcControlsInit(void)
|
||||
|
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -351,7 +352,7 @@ static void rescueAttainPosition()
|
|||
rescueYaw = constrainf(rescueYaw, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
|
||||
// rescueYaw is the yaw rate in deg/s to correct the heading error
|
||||
|
||||
const float rollMixAttenuator = constrainf(1.0f - ABS(rescueYaw) * 0.01f, 0.0f, 1.0f);
|
||||
const float rollMixAttenuator = constrainf(1.0f - fabsf(rescueYaw) * 0.01f, 0.0f, 1.0f);
|
||||
// less roll at higher yaw rates, no roll at 100 deg/s of yaw
|
||||
const float rollAdjustment = -rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator;
|
||||
// if rollMix = 100, the roll:yaw ratio is 1:1 at small angles, reducing linearly to zero when the yaw rate is 100 deg/s
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -656,7 +657,7 @@ void imuSetHasNewData(uint32_t dt)
|
|||
|
||||
bool imuQuaternionHeadfreeOffsetSet(void)
|
||||
{
|
||||
if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) {
|
||||
if ((abs(attitude.values.roll) < 450) && (abs(attitude.values.pitch) < 450)) {
|
||||
const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
|
||||
|
||||
offset.w = cos_approx(yaw/2);
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
|
@ -447,7 +448,7 @@ static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnable
|
|||
if (mixerConfig()->mixer_type == MIXER_LINEAR) {
|
||||
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
|
||||
} else {
|
||||
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + ABS(motorMix[i]), motorMix[i] - ABS(motorMix[i]));
|
||||
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + fabsf(motorMix[i]), motorMix[i] - fabsf(motorMix[i]));
|
||||
}
|
||||
motorMix[i] *= motorMixNormalizationFactor;
|
||||
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
|
@ -373,7 +374,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float calcHorizonLevelStrength()
|
|||
float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
|
||||
|
||||
// 0 at level, 90 at vertical, 180 at inverted (degrees):
|
||||
const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f;
|
||||
const float currentInclination = MAX(abs(attitude.values.roll), abs(attitude.values.pitch)) / 10.0f;
|
||||
|
||||
// horizonTiltExpertMode: 0 = leveling always active when sticks centered,
|
||||
// 1 = leveling can be totally off when inverted
|
||||
|
@ -480,8 +481,8 @@ static void handleCrashRecovery(
|
|||
&& fabsf(gyro.gyroADCf[FD_YAW]) < pidRuntime.crashRecoveryRate)) {
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
// check aircraft nearly level
|
||||
if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < pidRuntime.crashRecoveryAngleDeciDegrees
|
||||
&& ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < pidRuntime.crashRecoveryAngleDeciDegrees) {
|
||||
if (abs(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < pidRuntime.crashRecoveryAngleDeciDegrees
|
||||
&& abs(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < pidRuntime.crashRecoveryAngleDeciDegrees) {
|
||||
pidRuntime.inCrashRecoveryMode = false;
|
||||
BEEP_OFF;
|
||||
}
|
||||
|
|
|
@ -901,7 +901,7 @@ static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_
|
|||
int offset = larsonIndex - larsonParameters->currentIndex;
|
||||
static const int larsonLowValue = 8;
|
||||
|
||||
if (ABS(offset) > 1)
|
||||
if (abs(offset) > 1)
|
||||
return (larsonLowValue);
|
||||
|
||||
if (offset == 0)
|
||||
|
|
|
@ -219,7 +219,7 @@ int osdPrintFloat(char *buffer, char leadingSymbol, float value, char *formatStr
|
|||
}
|
||||
|
||||
value *= multiplier;
|
||||
const int scaledValueAbs = ABS(round ? lrintf(value) : value);
|
||||
const int scaledValueAbs = abs(round ? (int)lrintf(value) : (int)value);
|
||||
const int integerPart = scaledValueAbs / multiplier;
|
||||
const int fractionalPart = scaledValueAbs % multiplier;
|
||||
|
||||
|
|
|
@ -332,8 +332,8 @@ static void osdFormatCoordinate(char *buff, gpsCoordinateType_e coordinateType,
|
|||
gpsValue = (coordinateType == GPS_LONGITUDE) ? gpsSol.llh.lon : gpsSol.llh.lat;
|
||||
}
|
||||
|
||||
const int degreesPart = ABS(gpsValue) / GPS_DEGREES_DIVIDER;
|
||||
int fractionalPart = ABS(gpsValue) % GPS_DEGREES_DIVIDER;
|
||||
const int degreesPart = abs(gpsValue) / GPS_DEGREES_DIVIDER;
|
||||
int fractionalPart = abs(gpsValue) % GPS_DEGREES_DIVIDER;
|
||||
|
||||
switch (variantType) {
|
||||
#ifdef USE_GPS_PLUS_CODES
|
||||
|
@ -719,7 +719,7 @@ static void osdElementUpDownReference(osdElementParms_t *element)
|
|||
// Up/Down reference feature displays reference points on the OSD at Zenith and Nadir
|
||||
const float earthUpinBodyFrame[3] = {-rMat[2][0], -rMat[2][1], -rMat[2][2]}; //transforum the up vector to the body frame
|
||||
|
||||
if (ABS(earthUpinBodyFrame[2]) < SINE_25_DEG && ABS(earthUpinBodyFrame[1]) < SINE_25_DEG) {
|
||||
if (fabsf(earthUpinBodyFrame[2]) < SINE_25_DEG && fabsf(earthUpinBodyFrame[1]) < SINE_25_DEG) {
|
||||
float thetaB; // pitch from body frame to zenith/nadir
|
||||
float psiB; // psi from body frame to zenith/nadir
|
||||
char *symbol[2] = {"U", "D"}; // character buffer
|
||||
|
|
|
@ -28,6 +28,7 @@
|
|||
* Phobos- - Port of v2.0
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "platform.h"
|
||||
|
||||
|
@ -976,15 +977,15 @@ static void handleConnectionStateUpdate(const uint32_t timeStampMs)
|
|||
lostConnection();
|
||||
}
|
||||
|
||||
if ((receiver.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) {
|
||||
if ((receiver.connectionState == ELRS_TENTATIVE) && (abs(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) {
|
||||
gotConnection(timeStampMs); // detects when we are connected
|
||||
}
|
||||
|
||||
if ((receiver.timerState == ELRS_TIM_TENTATIVE) && ((timeStampMs - receiver.gotConnectionMs) > ELRS_CONSIDER_CONNECTION_GOOD_MS) && (ABS(pl.offsetDeltaUs) <= 5)) {
|
||||
if ((receiver.timerState == ELRS_TIM_TENTATIVE) && ((timeStampMs - receiver.gotConnectionMs) > ELRS_CONSIDER_CONNECTION_GOOD_MS) && (abs(pl.offsetDeltaUs) <= 5)) {
|
||||
receiver.timerState = ELRS_TIM_LOCKED;
|
||||
}
|
||||
|
||||
if ((receiver.connectionState == ELRS_CONNECTED) && (ABS(pl.offsetDeltaUs) > 10) && (pl.offsetUs >= 100) && (lqGet() <= minLqForChaos())) {
|
||||
if ((receiver.connectionState == ELRS_CONNECTED) && (abs(pl.offsetDeltaUs) > 10) && (pl.offsetUs >= 100) && (lqGet() <= minLqForChaos())) {
|
||||
lostConnection(); // SPI: resync when we're in chaos territory
|
||||
}
|
||||
}
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -183,7 +184,7 @@ static void updateBatteryBeeperAlert(void)
|
|||
|
||||
static bool isVoltageStable(void)
|
||||
{
|
||||
return ABS(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
|
||||
return abs(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
|
||||
}
|
||||
|
||||
static bool isVoltageFromBat(void)
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
@ -241,7 +242,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
|
|||
{
|
||||
int32_t absgps, deg, min;
|
||||
|
||||
absgps = ABS(mwiigps);
|
||||
absgps = abs(mwiigps);
|
||||
deg = absgps / GPS_DEGREES_DIVIDER;
|
||||
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
|
||||
min = absgps / GPS_DEGREES_DIVIDER; // minutes left
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -288,7 +289,7 @@ void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const e
|
|||
|
||||
uint32_t calcGpsDDMMmmm(int32_t value, bool isLong)
|
||||
{
|
||||
uint32_t absValue = ABS(value);
|
||||
uint32_t absValue = abs(value);
|
||||
uint16_t deg16 = absValue / GPS_DEGREES_DIVIDER;
|
||||
uint16_t min16 = (absValue - deg16 * GPS_DEGREES_DIVIDER) * 6 / 1000;
|
||||
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -242,7 +243,7 @@ bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
|
|||
static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
|
||||
{
|
||||
int32_t absgps, deg, min;
|
||||
absgps = ABS(mwiigps);
|
||||
absgps = abs(mwiigps);
|
||||
deg = absgps / GPS_DEGREES_DIVIDER;
|
||||
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
|
||||
min = absgps / GPS_DEGREES_DIVIDER; // minutes left
|
||||
|
@ -308,7 +309,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
|
|||
longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
|
||||
|
||||
// altitude (low order)
|
||||
altitudeLo = ABS(gpsSol.llh.altCm) / 10;
|
||||
altitudeLo = abs(gpsSol.llh.altCm) / 10;
|
||||
altitudeLoBcd = dec2bcd(altitudeLo % 100000);
|
||||
|
||||
// Ground course
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue