Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Minor performance update

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-05-17 11:47:37 +02:00
parent 7795514259
commit 500c6ab923
22 changed files with 52 additions and 44 deletions
Download patch file Download diff file Expand all files Collapse all files

13
src/main/common/maths.c
View file

@ -19,6 +19,7 @@
#include <string.h>
#include <math.h>
#include "arm_math.h"
#include "axis.h"
#include "maths.h"
#include "vector.h"
@ -95,7 +96,7 @@ float atan2_approx(float y, float x)
float acos_approx(float x)
{
float xa = fabsf(x);
float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
float result = fast_fsqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
if (x < 0.0f)
return M_PIf - result;
else
@ -200,7 +201,7 @@ float devVariance(stdev_t *dev)
float devStandardDeviation(stdev_t *dev)
{
return sqrtf(devVariance(dev));
return fast_fsqrtf(devVariance(dev));
}
float degreesToRadians(int16_t degrees)
@ -508,7 +509,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
for (int i = 0; i < 3; i++) {
result[i] = sqrtf(beta[i]);
result[i] = fast_fsqrtf(beta[i]);
}
return sensorCalibrationValidateResult(result);
@ -518,3 +519,9 @@ float bellCurve(const float x, const float curveWidth)
{
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
}
float fast_fsqrtf(const float value) {
float squirt;
arm_sqrt_f32(value, &squirt);
return squirt;
}

1
src/main/common/maths.h
View file

@ -176,3 +176,4 @@ float acos_approx(float x);
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 float value);

2
src/main/common/quaternion.h
View file

@ -142,7 +142,7 @@ static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, cons
static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
{
float mod = sqrtf(quaternionNormSqared(q));
float mod = fast_fsqrtf(quaternionNormSqared(q));
if (mod < 1e-6f) {
// Length is too small - re-initialize to zero rotation
result->q0 = 1;

2
src/main/common/vector.h
View file

@ -67,7 +67,7 @@ static inline float vectorNormSquared(const fpVector3_t * v)
static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v)
{
float length = sqrtf(vectorNormSquared(v));
float length = fast_fsqrtf(vectorNormSquared(v));
if (length != 0) {
result->x = v->x / length;
result->y = v->y / length;

10
src/main/flight/imu.c
View file

@ -241,7 +241,7 @@ static float imuGetPGainScaleFactor(void)
static void imuResetOrientationQuaternion(const fpVector3_t * accBF)
{
const float accNorm = sqrtf(vectorNormSquared(accBF));
const float accNorm = fast_fsqrtf(vectorNormSquared(accBF));
orientation.q0 = accBF->z + accNorm;
orientation.q1 = accBF->y;
@ -436,12 +436,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
// Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
// For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
// then we can safely use the "low angle" approximated version without loss of accuracy.
if (thetaMagnitudeSq < sqrtf(24.0f * 1e-6f)) {
if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) {
quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
}
else {
const float thetaMagnitude = sqrtf(thetaMagnitudeSq);
const float thetaMagnitude = fast_fsqrtf(thetaMagnitudeSq);
quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude);
deltaQ.q0 = cos_approx(thetaMagnitude);
}
@ -483,7 +483,7 @@ static float imuCalculateAccelerometerWeight(const float dT)
accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
}
const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
// Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we
// should not use measured accel for AHRS comp
@ -504,7 +504,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 = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT);
if (imuConfig()->acc_ignore_slope) {

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

@ -347,8 +347,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 = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
float stickDeflectionLength = fast_fsqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
float stickDeflectionExpoLength = fast_fsqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll
@ -362,8 +362,8 @@ static void applyTurtleModeToMotors(void) {
}
const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
(sqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
(fast_fsqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = fast_fsqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
if (cosPhi < cosThreshold) {
// Enforce either roll or pitch exclusively, if not on diagonal

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

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

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

@ -497,7 +497,7 @@ void processContinuousServoAutotrim(const float dT)
static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
static uint32_t servoMiddleUpdateCount;
const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF));
const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
const float targetRateMagnitude = getTotalRateTarget();
const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);

8
src/main/flight/wind_estimator.c
View file

@ -72,7 +72,7 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle)
}
*angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
}
return sqrtf(sq(xWindSpeed) + sq(yWindSpeed));
return fast_fsqrtf(sq(xWindSpeed) + sq(yWindSpeed));
}
void updateWindEstimator(timeUs_t currentTimeUs)
@ -133,7 +133,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
// estimate airspeed it using equation 6
float V = (sqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / sqrtf(diffLengthSq);
float V = (fast_fsqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / fast_fsqrtf(diffLengthSq);
fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X];
fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y];
@ -155,8 +155,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 = sqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z]));
float windLength = sqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z]));
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]));
if (windLength < prevWindLength + 2000) {
// TODO: Better filtering

2
src/main/io/gps_msp.c
View file

@ -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 = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
gpsSol.groundSpeed = fast_fsqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast));
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);

2
src/main/io/gps_naza.c
View file

@ -199,7 +199,7 @@ static bool NAZA_parse_gps(void)
gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm
gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm
gpsSol.numSat = _buffernaza.nav.satellites;
gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
// calculate gps heading from VELNE
gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f));

2
src/main/io/osd.c
View file

@ -3544,7 +3544,7 @@ static void osdFilterData(timeUs_t currentTimeUs) {
static timeUs_t lastRefresh = 0;
float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
if (lastRefresh) {

2
src/main/io/osd_common.c
View file

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

6
src/main/navigation/navigation.c
View file

@ -1933,7 +1933,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
posControl.actualState.agl.vel.x = newVelX;
posControl.actualState.agl.vel.y = newVelY;
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY));
// CASE 1: POS & VEL valid
if (estPosValid && estVelValid) {
@ -2067,7 +2067,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
*-----------------------------------------------------------*/
static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
{
return sqrtf(sq(deltaX) + sq(deltaY));
return fast_fsqrtf(sq(deltaX) + sq(deltaY));
}
static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
@ -3730,7 +3730,7 @@ static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, in
const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
*dist = fast_fsqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
*bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg

4
src/main/navigation/navigation_fixedwing.c
View file

@ -249,7 +249,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 = sqrtf(sq(posErrorX) + sq(posErrorY));
float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@ -272,7 +272,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 = sqrtf(sq(posErrorX) + sq(posErrorY));
distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint

6
src/main/navigation/navigation_multicopter.c
View file

@ -437,7 +437,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 = sqrtf(sq(newVelX) + sq(newVelY));
float newVelTotal = fast_fsqrtf(sq(newVelX) + sq(newVelY));
/*
* We override computed speed with max speed in following cases:
@ -497,7 +497,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float setpointX = posControl.desiredState.vel.x;
const float setpointY = posControl.desiredState.vel.y;
const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2));
const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2));
// Calculate velocity error
const float velErrorX = setpointX - measurementX;
@ -505,7 +505,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
// Calculate XY-acceleration limit according to velocity error limit
float accelLimitX, accelLimitY;
const float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY));
const float velErrorMagnitude = fast_fsqrtf(sq(velErrorX) + sq(velErrorY));
if (velErrorMagnitude > 0.1f) {
accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);

4
src/main/navigation/navigation_pos_estimator.c
View file

@ -164,7 +164,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 = sqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
isGlitching = false;
}
@ -640,7 +640,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 = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
const float gpsWeightScaler = 1.0f;

2
src/main/navigation/navigation_pos_estimator_flow.c
View file

@ -109,7 +109,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, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, fast_fsqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
}
DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));

2
src/main/programming/logic_condition.c
View file

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

10
src/main/sensors/acceleration.c
View file

@ -616,20 +616,20 @@ void updateAccExtremes(void)
if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
}
float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
float gforce = fast_fsqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2]));
if (gforce > acc.maxG) acc.maxG = gforce;
}
void accGetVibrationLevels(fpVector3_t *accVibeLevels)
{
accVibeLevels->x = sqrtf(acc.accVibeSq[X]);
accVibeLevels->y = sqrtf(acc.accVibeSq[Y]);
accVibeLevels->z = sqrtf(acc.accVibeSq[Z]);
accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
}
float accGetVibrationLevel(void)
{
return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
}
uint32_t accGetClipCount(void)

4
src/main/sensors/opflow.c
View file

@ -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 += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y]));
opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt;
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;
}
}

2
src/main/sensors/pitotmeter.c
View file

@ -223,7 +223,7 @@ STATIC_PROTOTHREAD(pitotThread)
//
// Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
// It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
} else {
performPitotCalibrationCycle();
pitot.airSpeed = 0;