diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 6f16d41acb0..49c8e8719c5 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1488,7 +1488,7 @@ static void loadSlowState(blackboxSlowState_t *slow) for (int i = 0; i < XYZ_AXIS_COUNT; i++) { #ifdef USE_WIND_ESTIMATOR - slow->wind[i] = getEstimatedWindSpeed(i); + slow->wind[i] = getEstimatedWindSpeed(i); // raw; Z is NEU #else slow->wind[i] = 0; #endif diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index e0ca0fba20d..4a889e0d4ba 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -153,7 +153,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { uint16_t windHeading = 0; // centidegrees const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading); - const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU + const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100; // NEU const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 512fd82dd89..dbdea46e35b 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -49,7 +49,7 @@ #define WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR 50 // good for wind speeds up to 30 m/s static bool hasValidWindEstimate = false; -static float estimatedWind[XYZ_AXIS_COUNT] = {0, 0, 0}; // wind velocity vectors in cm / sec in earth frame +static float estimatedWind[XYZ_AXIS_COUNT] = {0, 0, 0}; // wind velocity vectors in cm / sec in earth frame (Z: NEU) static float lastGroundVelocity[XYZ_AXIS_COUNT]; static float lastFuselageDirection[XYZ_AXIS_COUNT]; @@ -133,12 +133,12 @@ void updateWindEstimator(timeMs_t currentTimeMs) // Get current 3D velocity from GPS in cm/s relative to earth frame groundVelocity[X] = posEstimator.gps.vel.x; groundVelocity[Y] = posEstimator.gps.vel.y; - groundVelocity[Z] = posEstimator.gps.vel.z; + groundVelocity[Z] = posEstimator.gps.vel.z; // NEU // Fuselage direction in earth frame (radians) fuselageDirection[X] = HeadVecEFFiltered.x; fuselageDirection[Y] = -HeadVecEFFiltered.y; - fuselageDirection[Z] = HeadVecEFFiltered.z; + fuselageDirection[Z] = HeadVecEFFiltered.z; // NED // scrap our data and start over if we're taking too long (> 10s) to get a direction change if (MS2S(currentTimeMs - lastUseableAttitudeUpdateMs) > 10 || lastUseableAttitudeUpdateMs == 0) { @@ -188,7 +188,7 @@ void updateWindEstimator(timeMs_t currentTimeMs) float wind[XYZ_AXIS_COUNT]; wind[X] = (groundVelocitySum[X] - V * (costheta * fuselageDirectionSum[X] - sintheta * fuselageDirectionSum[Y])) * 0.5f; // equation 10 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 + wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f; // equation 12, NEU /* Spike filter used to filter out large spikes that can occur in the raw wind calcs. * Filter is based on a threshold between new wind updates and current estimated wind. diff --git a/src/main/flight/wind_estimator.h b/src/main/flight/wind_estimator.h index 52d7f470c11..e4464c1a059 100644 --- a/src/main/flight/wind_estimator.h +++ b/src/main/flight/wind_estimator.h @@ -27,6 +27,7 @@ bool isEstimatedWindSpeedValid(void); // wind velocity vectors in cm / sec relative to the earth frame +// X/Y (N/E) are convention-independent; Z axis is NEU (positive = updraft) float getEstimatedWindSpeed(int axis); // Returns the horizontal wind velocity as a magnitude in cm/s and, // optionally, its heading in EF in 0.01deg ([0, 360*100)). diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 61ebbe1f5cb..5276a7d0467 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3765,7 +3765,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_BLANK; bool valid = isEstimatedWindSpeedValid(); float verticalWindSpeed; - verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU + verticalWindSpeed = getEstimatedWindSpeed(Z); // NEU if (verticalWindSpeed < 0) { buff[1] = SYM_AH_DECORATION_DOWN; verticalWindSpeed = -verticalWindSpeed; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 5821b4e3c98..3b78f030e94 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -335,6 +335,7 @@ static float getWindEstimatedVirtualAirspeed(void) fpVector3_t windCorrectedVel; // Correct nav velocities with estimated wind velocities in earth frame + // Z: posControl.actualState.abs.vel.z and getEstimatedWindSpeed(Z) are both NEU for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { windCorrectedVel.v[axis] = posControl.actualState.abs.vel.v[axis] - getEstimatedWindSpeed(axis); }