Merge pull request #7746 from JulioCesarMatias/MathAddNewFunction

[maths.c] Add 2D and 3D Pythagorean functions

src/main/common/maths.c

@@ -532,7 +532,19 @@ float fast_fsqrtf(const double value) {
 #endif
     if (isnan(ret))
     {
-        return 0;
+        return 0.0f;
     }
     return ret;
 }
+
+// function to calculate the normalization (pythagoras) of a 2-dimensional vector
+float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
+{
+  return fast_fsqrtf(sq(firstElement) + sq(secondElement));
+}
+
+// function to calculate the normalization (pythagoras) of a 3-dimensional vector
+float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
+{
+  return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
+}

src/main/common/maths.h

@@ -183,3 +183,5 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 
 float bellCurve(const float x, const float curveWidth);
 float fast_fsqrtf(const double value);
+float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
+float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);

src/main/flight/imu.c

@@ -499,7 +499,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
     float accWeight_RateIgnore = 1.0f;
 
     if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) {
-        const float rotRateMagnitude = fast_fsqrtf(sq(imuMeasuredRotationBF.y) + sq(imuMeasuredRotationBF.z));
+        const float rotRateMagnitude = calc_length_pythagorean_2D(imuMeasuredRotationBF.y, imuMeasuredRotationBF.z);
         const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
 
         if (imuConfig()->acc_ignore_slope) {

src/main/flight/mixer.c

@@ -292,8 +292,8 @@ static void applyTurtleModeToMotors(void) {
         float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
         float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
 
-        float stickDeflectionLength = fast_fsqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
-        float stickDeflectionExpoLength = fast_fsqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
+        float stickDeflectionLength = calc_length_pythagorean_2D(stickDeflectionPitchAbs, stickDeflectionRollAbs);
+        float stickDeflectionExpoLength = calc_length_pythagorean_2D(stickDeflectionPitchExpo, stickDeflectionRollExpo);
 
         if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
             // If yaw is the dominant, disable pitch and roll

src/main/flight/pid.c

@@ -401,7 +401,7 @@ void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
 
 float getTotalRateTarget(void)
 {
-    return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
+    return calc_length_pythagorean_3D(pidState[FD_ROLL].rateTarget, pidState[FD_PITCH].rateTarget, pidState[FD_YAW].rateTarget);
 }
 
 float getAxisIterm(uint8_t axis)

src/main/flight/wind_estimator.c

@@ -72,7 +72,7 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle)
         }
         *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
     }
-    return fast_fsqrtf(sq(xWindSpeed) + sq(yWindSpeed));
+    return calc_length_pythagorean_2D(xWindSpeed, yWindSpeed);
 }
 
 void updateWindEstimator(timeUs_t currentTimeUs)
@@ -121,6 +121,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     fuselageDirectionDiff[Z] = fuselageDirection[Z] - lastFuselageDirection[Z];
 
     float diffLengthSq = sq(fuselageDirectionDiff[X]) + sq(fuselageDirectionDiff[Y]) + sq(fuselageDirectionDiff[Z]);
+    
     // Very small changes in attitude will result in a denominator
     // very close to zero which will introduce too much error in the
     // estimation.
@@ -133,7 +134,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
 
         // estimate airspeed it using equation 6
-        float V = (fast_fsqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / fast_fsqrtf(diffLengthSq);
+        float V = (calc_length_pythagorean_3D(groundVelocityDiff[X], groundVelocityDiff[Y], groundVelocityDiff[Z])) / fast_fsqrtf(diffLengthSq);
 
         fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X];
         fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y];
@@ -146,7 +147,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection));
         memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity));
 
-        float theta = atan2f(groundVelocityDiff[1], groundVelocityDiff[0]) - atan2f(fuselageDirectionDiff[1], fuselageDirectionDiff[0]);// equation 9
+        float theta = atan2f(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2f(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]);// equation 9
         float sintheta = sinf(theta);
         float costheta = cosf(theta);
 
@@ -155,8 +156,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11
         wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12
 
-        float prevWindLength = fast_fsqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z]));
-        float windLength = fast_fsqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z]));
+        float prevWindLength = calc_length_pythagorean_3D(estimatedWind[X], estimatedWind[Y], estimatedWind[Z]);
+        float windLength = calc_length_pythagorean_3D(wind[X], wind[Y], wind[Z]);
 
         if (windLength < prevWindLength + 2000) {
             // TODO: Better filtering
@@ -164,6 +165,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
             estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f;
             estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f;
         }
+
         lastUpdateUs = currentTimeUs;
         hasValidWindEstimate = true;
     }

src/main/io/gps_msp.c

@@ -89,7 +89,7 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
     gpsSol.velNED[X] = pkt->nedVelNorth;
     gpsSol.velNED[Y] = pkt->nedVelEast;
     gpsSol.velNED[Z] = pkt->nedVelDown;
-    gpsSol.groundSpeed = fast_fsqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
+    gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
     gpsSol.groundCourse = pkt->groundCourse / 10;   // in deg * 10
     gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
     gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);

src/main/io/osd_common.c

@@ -195,6 +195,6 @@ int16_t osdGet3DSpeed(void)
 {
     int16_t vert_speed = getEstimatedActualVelocity(Z);
     int16_t hor_speed = gpsSol.groundSpeed;
-    return (int16_t)fast_fsqrtf(sq(hor_speed) + sq(vert_speed));
+    return (int16_t)calc_length_pythagorean_2D(hor_speed, vert_speed);
 }
 #endif

src/main/navigation/navigation.c

@@ -1955,7 +1955,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
     posControl.actualState.agl.vel.x = newVelX;
     posControl.actualState.agl.vel.y = newVelY;
 
-    posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
+    posControl.actualState.velXY = calc_length_pythagorean_2D(newVelX, newVelY);
 
     // CASE 1: POS & VEL valid
     if (estPosValid && estVelValid) {
@@ -2085,7 +2085,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
  *-----------------------------------------------------------*/
 static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
 {
-    return fast_fsqrtf(sq(deltaX) + sq(deltaY));
+    return calc_length_pythagorean_2D(deltaX, deltaY);
 }
 
 static int32_t calculateBearingFromDelta(float deltaX, float deltaY)

src/main/navigation/navigation_fixedwing.c

@@ -266,7 +266,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
 
-    float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
+    float distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
 
     // Limit minimum forward velocity to 1 m/s
     float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@@ -290,7 +290,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
         // We have temporary loiter target. Recalculate distance and position error
         posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
         posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
-        distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
+        distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
     }
 
     // Calculate virtual waypoint

src/main/navigation/navigation_multicopter.c

@@ -435,7 +435,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
     float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
 
     // Scale velocity to respect max_speed
-    float newVelTotal = fast_fsqrtf(sq(newVelX) + sq(newVelY));
+    float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY);
 
     /*
      * We override computed speed with max speed in following cases:
@@ -501,7 +501,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     // Calculate XY-acceleration limit according to velocity error limit
     float accelLimitX, accelLimitY;
-    const float velErrorMagnitude = fast_fsqrtf(sq(velErrorX) + sq(velErrorY));
+    const float velErrorMagnitude = calc_length_pythagorean_2D(velErrorX, velErrorY);
+    
     if (velErrorMagnitude > 0.1f) {
         accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
         accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);

src/main/navigation/navigation_pos_estimator.c

@@ -163,7 +163,7 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
         predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
 
         /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
-        gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
+        gpsDistance = calc_length_pythagorean_2D(predictedGpsPosition.x - lastKnownGoodPosition.x, predictedGpsPosition.y - lastKnownGoodPosition.y);
         if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
             isGlitching = false;
         }
@@ -651,7 +651,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
             const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
             const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
             const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
-            const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
+            const float gpsPosResidualMag = calc_length_pythagorean_2D(gpsPosXResidual, gpsPosYResidual);
 
             //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
             const float gpsWeightScaler = 1.0f;

src/main/navigation/navigation_pos_estimator_flow.c

@@ -107,7 +107,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
         ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
         ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
 
-        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, fast_fsqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
+        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, calc_length_pythagorean_2D(flowResidualX, flowResidualY), positionEstimationConfig()->w_xy_flow_p);
     }
 
     DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));

src/main/programming/logic_condition.c

@@ -550,7 +550,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
-            return constrain(fast_fsqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX);
+            return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:

src/main/sensors/acceleration.c

@@ -584,7 +584,7 @@ void updateAccExtremes(void)
         if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
     }
 
-    float gforce = fast_fsqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
+    float gforce = calc_length_pythagorean_3D(acc.accADCf[X], acc.accADCf[Y], acc.accADCf[Z]);
     if (gforce > acc.maxG) acc.maxG = gforce;
 }
 

src/main/sensors/opflow.c

@@ -237,8 +237,8 @@ void opflowUpdate(timeUs_t currentTimeUs)
             else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) {
                 // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough
                 const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime;
-                opflowCalibrationBodyAcc += fast_fsqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
-                opflowCalibrationFlowAcc += fast_fsqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
+                opflowCalibrationBodyAcc += calc_length_pythagorean_2D(opflow.bodyRate[X], opflow.bodyRate[Y]);
+                opflowCalibrationFlowAcc += calc_length_pythagorean_2D(opflow.dev.rawData.flowRateRaw[X], opflow.dev.rawData.flowRateRaw[Y]) * invDt;
             }
         }