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

first good version

Browse source
This commit is contained in:
Sergey 2022-04-14 20:56:37 +03:00
parent 71e491fc12
commit 78c2060093
No known key found for this signature in database
GPG key ID: 4A7AC190CD92FE01
2 changed files with 8 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/fc_msp.c
View file

@ -21,6 +21,7 @@
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include "common/log.h"
#include "platform.h" #include "platform.h"
#include "blackbox/blackbox.h" #include "blackbox/blackbox.h"

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

@ -537,6 +537,9 @@ static void estimationPredict(estimationContext_t * ctx)
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight; posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
} }
if (ARMING_FLAG(SIMULATOR_MODE)) {
posEstimator.est.pos.z = baro.BaroAlt;
}
/* Prediction step: XY-axis */ /* Prediction step: XY-axis */
if ((ctx->newFlags & EST_XY_VALID)) { if ((ctx->newFlags & EST_XY_VALID)) {
@ -848,17 +851,16 @@ void initializePositionEstimator(void)
pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f); pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
} }
static bool isInitialized = false;
/** /**
* Update estimator * Update estimator
* Update rate: loop rate (>100Hz) * Update rate: loop rate (>100Hz)
*/ */
void updatePositionEstimator(void) void updatePositionEstimator(void)
{ {
if (ARMING_FLAG(SIMULATOR_MODE)) { // todo: why this const is inside function?
posEstimator.est.pos.z = baro.BaroAlt; // static bool isInitialized = false;
//return;
}
static bool isInitialized = false;
if (!isInitialized) { if (!isInitialized) {
initializePositionEstimator(); initializePositionEstimator();