ekf2: EKF vehicle_at_rest always require some rotation in addition to vibration metrics

dagar · dagar · commit 4cf8eb822673 · 2021-10-18T21:39:40.000-04:00
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -87,9 +87,8 @@ bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
 {
 	// detect if the vehicle is not moving when on ground
 	if (!_control_status.flags.in_air) {
-		if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
-		    || (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)
-		    || ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
+		if (((_vibe_metrics(1) * 4.0e4f > _params.is_moving_scaler) || (_vibe_metrics(2) * 2.1e2f > _params.is_moving_scaler))
+		    && ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
 
 			_time_last_move_detect_us = imu.time_us;
 		}

Original file line number	Diff line number	Diff line change
`@@ -87,9 +87,8 @@ bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)`
`87`	`87`	`{`
`88`	`88`	`// detect if the vehicle is not moving when on ground`
`89`	`89`	`if (!_control_status.flags.in_air) {`
`90`		`- if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)`
`91`		`- \|\| (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)`
`92`		`- \|\| ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {`
	`90`	`+ if (((_vibe_metrics(1) * 4.0e4f > _params.is_moving_scaler) \|\| (_vibe_metrics(2) * 2.1e2f > _params.is_moving_scaler))`
	`91`	`+ && ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {`
`93`	`92`
`94`	`93`	`_time_last_move_detect_us = imu.time_us;`
`95`	`94`	`}`