diff --git a/src/deck/drivers/src/flowdeck_v1v2.c b/src/deck/drivers/src/flowdeck_v1v2.c index bfd971ec46..75a24f7e10 100755 --- a/src/deck/drivers/src/flowdeck_v1v2.c +++ b/src/deck/drivers/src/flowdeck_v1v2.c @@ -120,7 +120,13 @@ static void flowdeckTask(void *param) flowMeasurement_t flowData; flowData.stdDevX = stdFlow; flowData.stdDevY = stdFlow; - flowData.dt = 0.01; + flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f; + // we do want to update dt every measurement and not only in the ones with detected motion, + // as we work with instantaneous gyro and velocity values in the update function + // (meaning assuming the current measurements over all of dt) + lastTime = usecTimestamp(); + + #if defined(USE_MA_SMOOTHING) // Use MA Smoothing @@ -151,8 +157,6 @@ static void flowdeckTask(void *param) // Push measurements into the estimator if flow is not disabled // and the PMW flow sensor indicates motion detection if (!useFlowDisabled && currentMotion.motion == 0xB0) { - flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f; - lastTime = usecTimestamp(); estimatorEnqueueFlow(&flowData); } } else { diff --git a/src/modules/src/kalman_core/mm_flow.c b/src/modules/src/kalman_core/mm_flow.c index 48f31b2624..0689ad64a3 100644 --- a/src/modules/src/kalman_core/mm_flow.c +++ b/src/modules/src/kalman_core/mm_flow.c @@ -26,6 +26,8 @@ #include "mm_flow.h" #include "log.h" +#define FLOW_RESOLUTION 0.1f //We do get the measurements in 10x the motion pixels (experimentally measured) + // TODO remove the temporary test variables (used for logging) static float predictedNX; static float predictedNY; @@ -38,9 +40,9 @@ void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *f // ~~~ Camera constants ~~~ // The angle of aperture is guessed from the raw data register and thankfully look to be symmetric - float Npix = 30.0; // [pixels] (same in x and y) + float Npix = 35.0; // [pixels] (same in x and y) //float thetapix = DEG_TO_RAD * 4.0f; // [rad] (same in x and y) - float thetapix = DEG_TO_RAD * 4.2f; + float thetapix = 0.71674f;// 2*sin(42/2); 42degree is the agnle of aperture, here we computed the corresponding ground length //~~~ Body rates ~~~ // TODO check if this is feasible or if some filtering has to be done float omegax_b = gyro->x * DEG_TO_RAD; @@ -71,32 +73,31 @@ void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *f } // ~~~ X velocity prediction and update ~~~ - // predics the number of accumulated pixels in the x-direction - float omegaFactor = 1.25f; + // predicts the number of accumulated pixels in the x-direction float hx[KC_STATE_DIM] = {0}; arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx}; - predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b); - measuredNX = flow->dpixelx; + predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegay_b); + measuredNX = flow->dpixelx*FLOW_RESOLUTION; // derive measurement equation with respect to dx (and z?) hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g)); hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g); //First update - kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX); + kalmanCoreScalarUpdate(this, &Hx, (measuredNX-predictedNX), flow->stdDevX*FLOW_RESOLUTION); // ~~~ Y velocity prediction and update ~~~ float hy[KC_STATE_DIM] = {0}; arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy}; - predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b); - measuredNY = flow->dpixely; + predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegax_b); + measuredNY = flow->dpixely*FLOW_RESOLUTION; // derive measurement equation with respect to dy (and z?) hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g)); hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g); // Second update - kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY); + kalmanCoreScalarUpdate(this, &Hy, (measuredNY-predictedNY), flow->stdDevY*FLOW_RESOLUTION); } /**