ekf2_test: use motion_planning for dynamic yaw emergency test

src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp

@@ -366,6 +366,13 @@ void SensorSimulator::setSensorDataFromTrajectory()
 				 -vel_body(0) * flow_sample.dt / distance_to_ground);
 		_flow.setData(flow_sample);
 	}
+
+	if (_gps.isRunning()) {
+		/* _gps.setAltitude(); */
+		/* _gps.setLatitude(); */
+		/* _gps.setLongitude(); */
+		_gps.setVelocity(vel_world);
+	}
 }
 
 void SensorSimulator::setGpsLatitude(const double latitude)

src/modules/ekf2/test/sensor_simulator/sensor_simulator.h

@@ -113,6 +113,8 @@ public:
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 
 	void simulateOrientation(Quatf orientation);
+	void setOrientation(const Quatf &orientation) { _R_body_to_world = Dcmf(orientation); }
+	void setOrientation(const Dcmf &orientation) { _R_body_to_world = orientation; }
 
 	void loadSensorDataFromFile(std::string filename);