only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning

and increase horizontal velocity variance to allow velocity states to move towards solution that is aligned with the newly set wind Signed-off-by: RomanBapst <[email protected]>
PX4 · Jul 3, 2024 · 16cf77d · 16cf77d
1 parent ec5f09d
commit 16cf77d
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp
@@ -60,6 +60,11 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction,
 	_information_events.flags.reset_wind_to_ext_obs = true;
 
 	resetWindTo(wind, wind_var);
+
+	// reset the horizontal velocity variances to allow the velocity states to be pulled towards
+	// a solution that is aligned with the newly set wind estimates
+	static constexpr float hor_vel_var = 25.0f;
+	P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
 }
 
 void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -536,8 +536,10 @@ void EKF2::Run()
 			}
 
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
-				_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
-								    vehicle_command.param4);
+				if (_ekf.control_status_flags().wind_dead_reckoning) {
+					_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
+									    vehicle_command.param4);
+				}
 			}
 		}
 	}