Copy implementation

Add msgs
2026-05-22 15:17:35 +08:00 · 2025-07-08 14:24:18 +09:00
parent 5a2b26f931
commit b67cc482d4
188 changed files with 3383 additions and 0 deletions
@@ -13,6 +13,7 @@ control_allocator start
 #
 # Start attitude controller.
 #
+fw_indi_pos_control start
 fw_rate_control start
 fw_att_control start
 fw_mode_manager start
@@ -17,6 +17,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_DYN_SOAR_CONTROL=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_MODE_MANAGER=y
@@ -206,6 +206,12 @@ set(msg_files
 	SensorsStatusImu.msg
 	SensorUwb.msg
 	SensorAirflow.msg
+	soaring_controller_heartbeat.msg
+	soaring_controller_position_setpoint.msg
+	soaring_controller_position.msg
+	soaring_controller_status.msg
+	soaring_controller_wind.msg
+	soaring_estimator_shear.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -0,0 +1,5 @@
+# SOARING CONTROLLER STATE, SERVES AS A HEARTBEAT IF CONTROLLER IS STILL PUBLISHING
+
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 heartbeat            # time since system start (microseconds) of the last run of soaring controller
@@ -0,0 +1,8 @@
+# SOARING CONTROLLER POSITION IN ENU SOARING FRAME
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] pos                            # POSITION VECTOR IN SOARING ENU FRAME
+float32[3] vel                            # VELOCITY VECTOR IN SOARING ENU FRAME
+float32[3] acc                            # ACCELERATION VECTOR IN SOARING ENU FRAME
+float32[4] att                            # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU
@@ -0,0 +1,12 @@
+# SOARING CONTROLLER POSITION SETPOINT
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] pos                             # position in ENU frame
+float32[3] vel                             # velocity in ENU frame
+float32[3] acc                             # acceleration in ENU frame
+float32[3] f_command                       # force command inside controller
+float32[3] m_command                       # moment command inside controller
+float32[3] w_err                           # rotation vector to target pose
+
+
@@ -0,0 +1,6 @@
+# SOARING CONTROLLER STATE, SERVES AS FLAG IF CONTROLLER IS STILL PUBLISHING
+
+uint64 timestamp			# time since system start (microseconds)
+
+bool soaring_controller_running     # if true, the soaring controller is expected to publish
+bool timeout_detected               # if true, the soaring controller has crashed
@@ -0,0 +1,11 @@
+# SOARING CONTROLLER WIND ESTIMATE, USED FOR INDI CONTROL
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] wind_estimate                            # WIND ESTIMATE IN ENU FRAME
+float32[3] wind_estimate_filtered                   # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER
+float32[3] position                                 # position of the current estimate in the soaring frame
+float32    airspeed                                 # current airspeed
+
+bool valid                                          # tell, if estimate is valid for shear estimator
+bool lock_params                                    # tell, if the shear estimator shall lock shear param values for soaring
@@ -0,0 +1,30 @@
+# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32 vx                  # maximum wind in x-direction
+float32 vy                  # maximum wind in y-direction
+float32 bx                  # wind offset in x-direction
+float32 by                  # wind offset in y-direction
+float32 h                   # vertical position of shear layer in soaring frame
+float32 a                   # shear strength
+
+float32 sigma_vx            # covariance of vx
+float32 sigma_vy            # covariance of vy
+float32 sigma_bx            # covariance of bx
+float32 sigma_by            # covariance of by
+float32 sigma_h             # covariance of h
+float32 sigma_a             # covariance of a
+
+float32 coeff_0             # offset vertical wind
+float32 coeff_1             # linear coeff vertical wind
+float32 coeff_2             # quadratic coeff vertical wind
+
+float32 v_max               # discrete value of v_max (shear velocity), identifier for appropriate trajectrory
+float32 alpha               # discrete value of alpha (shear strength), identifier for appropriate trajectrory
+float32 h_ref               # discrete value of h_ref (shear location)
+float32 psi                 # discrete value of shear heading
+float32 aspd                # airspeed identifier for appropriate trajectrory
+
+bool soaring_feasible       # plausibility check
+uint64 reset_counter        # filter reset counter
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#add_subdirectory(launchdetection)
+
+px4_add_module(
+	MODULE modules__fw_dyn_soar_control
+	MAIN fw_dyn_soar_control
+	STACK_MAIN 4096
+	SRCS
+		FixedwingPositionINDIControl.cpp
+		FixedwingPositionINDIControl.hpp
+	DEPENDS
+	)
@@ -0,0 +1,434 @@
+/**
+ * Implementation of a generic incremental position controller
+ * using incremental nonlinear dynamic inversion and differential flatness
+ * for a fixed wing aircraft during dynamic soaring cycles.
+ * The controller directly outputs actuator deflections for ailerons, elevator and rudder.
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+// use inclusion guards
+#ifndef FIXEDWINGPOSITIONINDICONTROL_HPP_
+#define FIXEDWINGPOSITIONINDICONTROL_HPP_
+
+#include <float.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+#include "fw_att_control/ecl_pitch_controller.h"
+#include "fw_att_control/ecl_roll_controller.h"
+#include "fw_att_control/ecl_wheel_controller.h"
+#include "fw_att_control/ecl_yaw_controller.h"
+#include <lib/ecl/geo/geo.h>
+#include <lib/l1/ECL_L1_Pos_Controller.hpp>
+#include <lib/landing_slope/Landingslope.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/airflow_aoa.h>
+#include <uORB/topics/airflow_slip.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/soaring_controller_heartbeat.h>
+#include <uORB/topics/soaring_controller_position_setpoint.h>
+#include <uORB/topics/soaring_controller_position.h>
+#include <uORB/topics/soaring_controller_status.h>
+#include <uORB/topics/soaring_controller_wind.h>
+#include <uORB/topics/soaring_estimator_shear.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/debug_value.h>
+#include <uORB/topics/wind.h>
+#include <uORB/uORB.h>
+
+
+
+using namespace time_literals;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector;
+using matrix::Matrix3f;
+using matrix::Vector3f;
+
+
+class FixedwingPositionINDIControl final : public ModuleBase<FixedwingPositionINDIControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FixedwingPositionINDIControl();
+	~FixedwingPositionINDIControl() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	orb_advert_t	_mavlink_log_pub{nullptr};
+
+	// make the main task run, whenever a new body rate becomes available
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+    // Subscriptions
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};						// vehicle status
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};             // airspeed
+    uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)};                           // angle of attack
+    uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)};                         // angle of sideslip
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};     	// linear acceleration
+    uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};     // local NED position
+    uORB::Subscription _home_position_sub{ORB_ID(home_position)};						// home position
+    uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};                 // vehicle attitude
+    uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel
+	uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)};			// vehicle status flags
+	uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)};	// shear params for trajectory selection
+	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
+
+    // Publishers
+	uORB::Publication<actuator_controls_s>							_actuators_0_pub;
+	uORB::Publication<vehicle_attitude_setpoint_s>					_attitude_sp_pub;
+	uORB::Publication<vehicle_rates_setpoint_s>						_angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<vehicle_angular_acceleration_setpoint_s>		_angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)};
+	uORB::PublicationMulti<rate_ctrl_status_s>						_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
+	uORB::Publication<soaring_controller_heartbeat_s>				_soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)};
+	uORB::Publication<soaring_controller_position_setpoint_s>		_soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
+	uORB::Publication<soaring_controller_position_s>				_soaring_controller_position_pub{ORB_ID(soaring_controller_position)};
+	uORB::Publication<soaring_controller_wind_s>					_soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)};
+	uORB::Publication<offboard_control_mode_s>						_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
+	uORB::Publication<debug_value_s>								_debug_value_pub{ORB_ID(debug_value)};
+
+    // Message structs
+	vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
+	actuator_controls_s			_actuators {};			// actuator commands
+	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
+	rc_channels_s				_rc_channels {};		///< rc channels
+	vehicle_local_position_s	_local_pos {};			///< vehicle local position
+	vehicle_acceleration_s		_acceleration {};		///< vehicle acceleration
+	vehicle_attitude_s			_attitude {};			///< vehicle attitude
+	vehicle_attitude_setpoint_s	_attitude_sp {};		///< vehicle attitude setpoint
+	vehicle_angular_velocity_s 	_angular_vel {};		///< vehicle angular velocity
+	vehicle_rates_setpoint_s 	_angular_vel_sp {};		///< vehicle angular velocity setpoint
+	vehicle_angular_acceleration_s	_angular_accel {};	///< vehicle angular acceleration
+	home_position_s				_home_pos {};			///< home position
+	map_projection_reference_s 	_global_local_proj_ref{};
+	vehicle_control_mode_s		_control_mode {};		///< control mode
+	offboard_control_mode_s		_offboard_control_mode {};	///< offboard control mode
+	vehicle_status_s		    _vehicle_status {};		///< vehicle status
+	soaring_controller_status_s	_soaring_controller_status {};	///< soaring controller status
+	soaring_controller_heartbeat_s	_soaring_controller_heartbeat{};	///< soaring controller hrt
+	soaring_controller_position_setpoint_s	_soaring_controller_position_setpoint{};	///< soaring controller pos setpoint
+	soaring_controller_position_s	_soaring_controller_position{};	///< soaring controller pos
+	soaring_controller_wind_s	_soaring_controller_wind{};	///< soaring controller wind
+	soaring_estimator_shear_s	_soaring_estimator_shear{}; ///< soaring estimator shear
+	debug_value_s				_debug_value{};			// slip angle
+
+	// parameter struct
+	DEFINE_PARAMETERS(
+		// aircraft params
+		(ParamFloat<px4::params::DS_INERTIA_ROLL>) _param_fw_inertia_roll,
+		(ParamFloat<px4::params::DS_INERTIA_PITCH>) _param_fw_inertia_pitch,
+		(ParamFloat<px4::params::DS_INERTIA_YAW>) _param_fw_inertia_yaw,
+		(ParamFloat<px4::params::DS_INERTIA_RP>) _param_fw_inertia_rp,
+		(ParamFloat<px4::params::DS_MASS>) _param_fw_mass,
+		(ParamFloat<px4::params::DS_WING_AREA>) _param_fw_wing_area,
+		(ParamFloat<px4::params::DS_RHO>) _param_rho,
+		// aerodynamic params (INDI)
+		(ParamFloat<px4::params::DS_C_L0>) _param_fw_c_l0,
+		(ParamFloat<px4::params::DS_C_L1>) _param_fw_c_l1,
+		(ParamFloat<px4::params::DS_C_D0>) _param_fw_c_d0,
+		(ParamFloat<px4::params::DS_C_D1>) _param_fw_c_d1,
+		(ParamFloat<px4::params::DS_C_D2>) _param_fw_c_d2,
+		(ParamFloat<px4::params::DS_C_B1>) _param_fw_c_b1,
+		(ParamFloat<px4::params::DS_AOA_OFFSET>) _param_aoa_offset,
+		(ParamFloat<px4::params::DS_STALL_SPEED>) _param_stall_speed,
+		// position PD control params
+		(ParamFloat<px4::params::DS_LIN_K_X>) _param_lin_k_x,
+		(ParamFloat<px4::params::DS_LIN_K_Y>) _param_lin_k_y,
+		(ParamFloat<px4::params::DS_LIN_K_Z>) _param_lin_k_z,
+		(ParamFloat<px4::params::DS_LIN_C_X>) _param_lin_c_x,
+		(ParamFloat<px4::params::DS_LIN_C_Y>) _param_lin_c_y,
+		(ParamFloat<px4::params::DS_LIN_C_Z>) _param_lin_c_z,
+		(ParamFloat<px4::params::DS_LIN_FF_X>) _param_lin_ff_x,
+		(ParamFloat<px4::params::DS_LIN_FF_Y>) _param_lin_ff_y,
+		(ParamFloat<px4::params::DS_LIN_FF_Z>) _param_lin_ff_z,
+		// attitude PD control params
+		(ParamFloat<px4::params::DS_ROT_K_ROLL>) _param_rot_k_roll,
+		(ParamFloat<px4::params::DS_ROT_K_PITCH>) _param_rot_k_pitch,
+		(ParamFloat<px4::params::DS_ROT_FF_YAW>) _param_rot_ff_yaw,
+		(ParamFloat<px4::params::DS_ROT_C_ROLL>) _param_rot_c_roll,
+		(ParamFloat<px4::params::DS_ROT_C_PITCH>) _param_rot_c_pitch,
+		(ParamFloat<px4::params::DS_ROT_P_YAW>) _param_rot_p_yaw,
+		// low-level controller params (INDI)
+		(ParamFloat<px4::params::DS_K_ACT_ROLL>) _param_k_act_roll,
+		(ParamFloat<px4::params::DS_K_ACT_PITCH>) _param_k_act_pitch,
+		(ParamFloat<px4::params::DS_K_DAMP_ROLL>) _param_k_damping_roll,
+		(ParamFloat<px4::params::DS_K_DAMP_PITCH>) _param_k_damping_pitch,
+		// location params
+		(ParamFloat<px4::params::DS_ORIGIN_LAT>) _param_origin_lat,
+		(ParamFloat<px4::params::DS_ORIGIN_LON>) _param_origin_lon,
+		(ParamFloat<px4::params::DS_ORIGIN_ALT>) _param_origin_alt,
+		// loiter params
+		(ParamInt<px4::params::DS_LOITER>) _param_loiter,
+		(ParamInt<px4::params::DS_W_HEADING>) _param_shear_heading,
+		(ParamFloat<px4::params::DS_W_HEIGHT>) _param_shear_height,
+		// thrust params
+		(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
+		// force saturation
+		(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
+		// hardcoded trajectory center
+		(ParamInt<px4::params::DS_SWITCH_ORI_HC>) _param_switch_origin_hardcoded,
+		// manual switch if manual feedthrough is used, only active in STIL mode
+		(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
+		// manual switch if manual feedthrough is used, REMOVE!!!
+		(ParamInt<px4::params::DS_SWITCH_CLOOP>) _param_switch_cloop,
+		// manual switch if we are in SITL mode
+		(ParamInt<px4::params::DS_SWITCH_SITL>) _param_switch_sitl
+
+	)
+
+
+	perf_counter_t	_loop_perf;				///< loop performance counter
+
+	// estimator reset counters
+	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
+	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
+
+
+	// Update our local parameter cache.
+	int		parameters_update();
+
+	// Update subscriptions
+	void        wind_poll();
+	void		airspeed_poll();
+	void		airflow_aoa_poll();
+	void 		airflow_slip_poll();
+
+	void		vehicle_local_position_poll();
+	void		vehicle_acceleration_poll();
+	void		vehicle_attitude_poll();
+	void		vehicle_angular_velocity_poll();
+	void		vehicle_angular_acceleration_poll();
+	void		actuator_controls_poll();
+
+	void		control_update();
+	void 		manual_control_setpoint_poll();
+	void		rc_channels_poll();
+	void		vehicle_command_poll();
+	void		vehicle_control_mode_poll();
+	void		vehicle_status_poll();
+	void		soaring_controller_status_poll();
+	void		soaring_estimator_shear_poll();
+
+	//
+	void		status_publish();
+
+	const static size_t _num_basis_funs = 16;		// number of basis functions used for the trajectory approximation
+
+	// controller methods
+	void _compute_trajectory_transform();			// compute the transform between trajectory frame and ENU frame (soaring frame) based on shear params
+	void _select_loiter_trajectory();				// select the correct loiter trajectory based on available energy
+	void _select_soaring_trajectory();				// select the correct loiter trajectory based on available energy
+	void _read_trajectory_coeffs_csv(char *filename);				// read in the correct coefficients of the appropriate trajectory
+	float _get_closest_t(Vector3f pos);				// get the normalized time, at which the reference path is closest to the current position
+	Vector3f _compute_wind_estimate();				// compute a wind estimate to be used only inside the controller
+	Vector3f _compute_wind_estimate_EKF();			// compute a wind estimate to be used only as a measurement for the shear estimator
+	void _set_wind_estimate(Vector3f wind);
+	void _set_wind_estimate_EKF(Vector3f wind);
+	Vector<float, _num_basis_funs> _get_basis_funs(float t=0);			// compute the vector of basis functions at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d_dt_basis_funs(float t=0);		// compute the vector of basis function gradients at normalized time t in [0,1]
+	Vector<float, _num_basis_funs> _get_d2_dt2_basis_funs(float t=0);	// compute the vector of basis function curvatures at normalized time t in [0,1]
+	void _load_basis_coefficients();									// load the coefficients of the current path approximation
+	Vector3f _get_position_ref(float t=0);								// get the reference position on the current path, at normalized time t in [0,1]
+	Vector3f _get_velocity_ref(float t=0, float T=1);					// get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_acceleration_ref(float t=0, float T=1);				// get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Quatf _get_attitude_ref(float t=0, float T=1);						// get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_velocity_ref(float t=0, float T=1);			// get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Vector3f _get_angular_acceleration_ref(float t=0, float T=1);		// get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T
+	Quatf _get_attitude(Vector3f vel, Vector3f f);						// get the attitude to produce force f while flying with velocity vel
+	Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref);
+	Vector3f _compute_INDI_stage_2(Vector3f ctrl);
+	Vector3f _compute_actuator_deflections(Vector3f ctrl);
+
+	// helper methods
+	void _reverse(char* str, int len);						// reverse a string of length 'len'
+	int _int_to_str(int x, char str[], int d);				// convert an integer x into a string of length d
+	void _float_to_str(float n, char* res, int afterpoint);	// convert float to string
+
+
+	// control variables
+	Vector<float, _num_basis_funs> _basis_coeffs_x = {};				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_y = {};				// coefficients of the current path
+	Vector<float, _num_basis_funs> _basis_coeffs_z = {};				// coefficients of the current path
+	Vector3f _alpha_sp;
+	Vector3f _wind_estimate;											// wind estimate used internally in the controller
+	Vector3f _wind_estimate_EKF;										// wind estimate only used in the wind estimator
+	Matrix3f _K_x;
+	Matrix3f _K_v;
+	Matrix3f _K_a;
+	Matrix3f _K_q;
+	Matrix3f _K_w;
+	Vector3f _pos;		// current position
+	Vector3f _vel;		// current velocity
+	Vector3f _acc;		// current acceleration
+	Quatf _att;			// attitude quaternion
+	Vector3f _omega;	// angular rate vector
+	Vector3f _alpha;	// angular acceleration vector
+	float _k_ail;
+    float _k_ele;
+    float _k_d_roll;
+    float _k_d_pitch;
+	hrt_abstime _last_run{0};
+
+	// controller frequency
+	const float _sample_frequency = 200.f;
+	// Low-Pass filters stage 1
+	const float _cutoff_frequency_1 = 20.f;
+	math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// linear acceleration
+	math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// force command
+	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
+	// smoothing filter to reject HF noise in control output
+	const float _cutoff_frequency_smoothing = 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
+	math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}};	// force command stage 1
+	math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10};	// rudder command
+	// Low-Pass filters stage 2
+	const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
+	math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// filter to match acceleration processing delay
+	math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}};	// body rates
+	// Low-Pass filter for wind estimate
+	const float _cutoff_frequency_wind = 1.f;
+	math::LowPassFilter2p _lp_filter_wind[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}};	// wind_estimate inside controller
+	math::LowPassFilter2p _lp_filter_wind_EKF[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}};	// wind_estimate for EKF
+	uint _counter = 0;
+	hrt_abstime _last_time{0};
+
+	// parameter variables
+	Matrix3f _inertia {};
+	float _mass;
+	float _area;
+	float _rho;
+	float _C_L0;
+	float _C_L1;
+	float _C_D0;
+	float _C_D1;
+	float _C_D2;
+	float _C_B1;
+	float _aoa_offset;
+	float _stall_speed;
+	// trajectory origin in WGS84
+	float _origin_lat;
+	float _origin_lon;
+	float _origin_alt;
+	// trajectory origin in current NED local frame
+	float _origin_N;
+	float _origin_E;
+	float _origin_D;
+	// wind shear parameters
+	float _shear_v_max;
+	float _shear_alpha;
+	float _shear_energy;
+	float _shear_h_ref;
+	float _shear_heading;
+	float _shear_aspd;
+	// loiter circle
+	int _loiter;
+	// thrust
+	float _thrust;
+	float _thrust_pos;
+	// ==================
+	// controler switches
+	// ==================
+	// controller mode
+	bool _switch_manual;
+	// soaring mode
+	bool _switch_cl_soaring;
+	// force limit
+	bool _switch_saturation;
+	// use shear height from estimator
+	bool _switch_origin_hardcoded;
+	//
+	bool _switch_sitl;
+	//
+	bool _soaring_feasible;
+
+
+	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
+	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
+	float _true_airspeed{0.0f};
+	float _cal_airspeed{0.0f};
+
+	bool _aoa_valid{false};				///< flag if a valid AoA estimate exists
+	hrt_abstime _aoa_last_valid{0};			///< last time Aoa was received. Used to detect timeouts.
+	float _aoa{0.0f};
+
+	bool _slip_valid{false};				///< flag if a valid AoA estimate exists
+	hrt_abstime _slip_last_valid{0};			///< last time Aoa was received. Used to detect timeouts.
+	float _slip{0.0f};
+
+	// vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength
+	const static size_t _gridsize = 11;
+
+
+	// helper variables
+	Dcmf _R_ned_to_enu;	// rotation matrix from NED to ENU frame
+	Dcmf _R_enu_to_ned;	// rotation matrix from ENU to NED frame
+	Dcmf _R_trajec_to_enu; // rotation matrix from trajectory frame to ENU frame
+	Dcmf _R_enu_to_trajec; // rotation matrix from ENU frame to trajectory frame
+	Vector3f _vec_enu_to_trajec; // 3D vector from ENU origin to trajectory frame origin (expressed in ENU)
+	Vector3f _zero_crossing_local_pos;	// vector denoting the zero crossing of the trajectories in NED frame
+	Vector3f _f_command {};
+	Vector3f _m_command {};
+	Vector3f _w_err {};
+	hrt_abstime _last_time_trajec{0};
+
+};
+
+
+
+#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_
@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_DYN_SOAR_CONTROL
+	bool "fw_dyn_soar_control"
+	default n
+	---help---
+		Enable support for fw_dyn_soar_control
+
+menuconfig USER_FW_DYN_SOAR_CONTROL
+	bool "fw_dyn_soar_control running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_DYN_SOAR_CONTROL
+	---help---
+		Put fw_dyn_soar_control in userspace memory
@@ -0,0 +1,657 @@
+/**
+ * @file fw_dyn_soar_control_params.c
+ *
+ * Parameters defined by the INDI position controller
+ *
+ * @author Marvin Harms <marv@teleport.ch>
+ */
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * inertia around body x-axis
+ * 
+ * This is the inertia of the aircraft, used for the INDI
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f);
+
+/**
+ * inertia around body y-axis
+ * 
+ * This is the inertia of the aircraft, used for the INDI
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f);
+
+/**
+ * inertia around body z-axis
+ * 
+ * This is the inertia of the aircraft, used for the INDI
+ * 
+ * @unit kg
+ * @min 0.01
+ * @max 10
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f);
+
+/**
+ * inertia tensor term in body xz-axis (roll-yaw coupling)
+ * 
+ * This is the inertia of the aircraft, used for the INDI
+ * 
+ * @unit kg
+ * @min -0.5
+ * @max 0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f);
+
+/**
+ * total takeoff mass
+ * 
+ * This is the mass of the aircraft, used for the INDI
+ * 
+ * @unit kg
+ * @min 1.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_MASS, 1.4f);
+
+/**
+ * total wing area used for lift and drag computation
+ * 
+ * @unit m^2
+ * @min 0.1
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f);
+
+
+/**
+ * air density used for lift and drag computation
+ * 
+ * @unit 
+ * @min 0.5
+ * @max 1.225
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_RHO, 1.223f);
+
+/**
+ * estimated lift coefficients used for lift and drag computation
+ * 
+ * Used as L = C_l0 + C_l1*alpha,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f);
+
+/**
+ * estimated lift coefficients used for lift and drag computation
+ * 
+ * Used as L = C_l0 + C_l1*alpha,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f);
+
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ * 
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f);
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ * 
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f);
+
+/**
+ * estimated drag coefficients used for lift and drag computation
+ * 
+ * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max 100
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f);
+
+/**
+ * estimated sideslip sensitivity coefficients used for wind estimation
+ * 
+ * Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1,
+ * where alpha is the angle of attack.
+ * 
+ * @unit 
+ * @min -100
+ * @max -0.01
+ * @decimal 4
+ * @increment 0.0001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f);
+
+/**
+ * offset angle between body frame (Pixhawk) and the wing chord
+ * 
+ * Used to compute the AoA
+ * 
+ * @unit rad
+ * @min 0
+ * @max 0.1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f);
+
+/**
+ * stall speed of the aircraft
+ * 
+ * @unit m/s
+ * @min 5
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f);
+
+
+// ========================================================
+// =================== CONTROL GAINS ======================
+// ========================================================
+/**
+ * control gain of position PD-controller (body x-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f);
+
+/**
+ * control gain of position PD-controller (body y-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f);
+
+/**
+ * control gain of position PD-controller (body z-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body x-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body y-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f);
+
+/**
+ * normalized damping coefficient of position PD-controller (body z-direction)
+ * 
+* @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body x-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body y-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f);
+
+/**
+ * acceleration feedback gain of position PD-controller (body z-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f);
+
+/**
+ * control gain of attitude PD-controller (body roll-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f);
+
+/**
+ * control gain of attitude PD-controller (body pitch-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f);
+
+/**
+ * rudder turn coordination FF-gain
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f);
+
+/**
+ * normalized damping coefficient of attitude PD-controller (body roll-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f);
+
+/**
+ * normalized damping coefficient of attitude PD-controller (body pitch-direction)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f);
+
+/**
+ * rudder turn coordination P-gain
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f);
+
+
+// =============================
+// low level INDI control params
+// =============================
+
+/**
+ * roll gain of K_ACT (actuator deflection gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f);
+
+/**
+ * pitch gain of K_ACT (actuator deflection gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f);
+
+
+/**
+ * roll gain of K_ACT_DAMPING (actuator damping gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f);
+
+/**
+ * pitch gain of K_ACT_DAMPING (actuator damping gain)
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 3
+ * @increment 0.001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f);
+
+
+// ===================================================
+// ==============  trajectory center =================
+// ===================================================
+
+/**
+ * latitude of trajectory start point (WGS84)
+ * 
+ * @unit 
+ * @min -90
+ * @max 90
+ * @decimal 7
+ * @increment 0.0000001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f);
+
+/**
+ * longitude of trajectory start point (WGS84)
+ * 
+ * @unit 
+ * @min -180
+ * @max 180
+ * @decimal 7
+ * @increment 0.0000001
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f);
+
+/**
+ * altitude of trajectory start point (WGS84)
+ * 
+ * @unit 
+ * @min 0
+ * @max 2000
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f);
+
+// ======================================================
+// ==============  loiter circle number =================
+// ======================================================
+
+/**
+ * integer in {0,1,2,3,4,5} defining the loiter trajectory
+ * 
+ * @unit 
+ * @min 0
+ * @max 7
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_LOITER, 0);
+
+// ======================================================
+// ============ wind shear heading ======================
+// ======================================================
+
+/**
+ * integer defining wind heading
+ * 
+ * @unit deg
+ * @min -180
+ * @max 180
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_W_HEADING, 0);
+
+// ======================================================
+// ======= wind shear height in soaring frame ===========
+// ======================================================
+
+/**
+ * float defining wind shear vertical postition in soaring frame
+ * 
+ * @unit 
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
+
+// ==============================================
+// ============== engine thrust =================
+// ==============================================
+/**
+ * float in [0,1] corresponding to the engine thrust
+ * 
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_FLOAT(DS_THRUST, 0);
+
+// ======================================================
+// ============= controller force saturation ============
+// ======================================================
+
+/**
+ * integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation
+ * 
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
+
+
+// ======================================================
+// ============= hardcoded trajectory center ============
+// ======================================================
+
+/**
+ * integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1);
+
+// =================================================
+// ============= loiter trajectory test ============
+// =================================================
+
+/**
+ * integer in {0,1} defining if the loiter circle defined by param DS_LOITER shall be used, 0=soaring, 1=loiter
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1);
+
+// ====================================================
+// ============= manual feedthrough switch ============
+// ====================================================
+
+/**
+ * integer in {0,1} defining if we are using manual feedthrough, only used in SITL mode
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1);
+
+// =====================================================
+// ============= open loop / closed loop DS ============
+// =====================================================
+
+/**
+ * integer in {0,1} defining if the shear parameters are changed by the estimator while soaring (closed loop). 0=fixed shear, 1=changing shear
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_CLOOP, 0);
+
+// =====================================================
+// ============= open loop / closed loop DS ============
+// =====================================================
+
+/**
+ * integer in {0,1} defining if we are running the controller in SITL. 0=hardware, 1=sitl
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_SITL, 0);
@@ -0,0 +1,3 @@
+20.249834,-1498.036199,8991.961311,-24958.956623,41340.383016,-41631.426333,17050.379042,19263.586599,-39432.861865,25757.956616,11245.661203,-42344.771826,46970.231532,-29988.424357,10855.560938,-1677.368349
+-7.218114,5279.878939,-27707.912587,70515.279933,-109233.570552,102935.722143,-36476.002615,-51554.017800,94917.464988,-58786.945547,-27934.800286,100170.063042,-114150.638209,78214.058653,-32445.366144,6339.659956
+-3.934978,2391.325102,-14411.271810,42203.244020,-76716.253156,88945.359914,-51569.236117,-25572.454067,87611.185180,-77472.095071,-5984.618456,99770.259330,-135165.631325,101511.221087,-44631.608657,9162.447954
@@ -0,0 +1,3 @@
+30.389860,912.982507,-2859.936980,3349.275775,-415.369310,-3481.089396,3363.872340,1179.921845,-4478.256881,1837.357593,3498.540940,-4492.878780,-393.125759,5151.515013,-4941.188034,1720.463927
+-2.598681,6191.674693,-33961.873408,91771.100021,-153409.649369,160707.118624,-75518.823722,-63886.003518,154241.187907,-115027.482925,-29953.909268,169169.609712,-207234.688426,146197.933872,-61050.853957,11854.416042
+-5.175201,6337.029321,-35104.159570,93181.991417,-150770.497726,150093.399535,-61550.310528,-68258.178502,141363.130375,-95038.720689,-36247.135711,151579.287214,-176414.846548,120921.610558,-49646.077608,9643.082070
@@ -0,0 +1,3 @@
+32.051371,1650.704144,-6598.531219,12390.119252,-13493.269501,7461.379871,1207.142418,-5970.998355,5109.589560,-2016.879398,-1051.211663,4872.887766,-9270.656434,10649.395680,-7172.952936,2187.636700
+-2.536588,5861.765277,-31409.143579,83398.251187,-137714.029102,143429.216873,-67935.630936,-55607.262617,137500.343143,-105281.108656,-23844.091823,152076.782007,-190543.222950,136636.797526,-57806.608731,11327.398362
+-5.904772,4632.654322,-26493.025191,72582.753211,-121329.073480,125497.496032,-56176.364936,-52937.280101,119529.487901,-84942.741316,-26808.719959,129359.716901,-154410.364036,107437.399311,-44588.163669,8729.336822
@@ -0,0 +1,3 @@
+31.550461,1802.021465,-7280.950015,13535.491205,-13709.001555,4905.090872,6059.191197,-9051.239430,2193.646055,5653.911056,-5994.939291,187.142042,3940.145313,-3036.799806,467.894600,264.261964
+-1.408562,1530.681950,-7030.394025,17659.485566,-29768.713956,34377.591980,-22054.294715,-6577.428854,34071.279368,-35600.224927,3176.621270,41037.278387,-63095.706736,50947.639967,-23488.896320,4861.320938
+-6.656401,3614.186108,-20979.720257,57857.341097,-96439.348021,98059.213352,-40613.389558,-45559.747550,93161.401353,-60017.407117,-27238.757931,98854.244272,-108993.602048,70675.872603,-27268.100397,4939.680351
@@ -0,0 +1,3 @@
+25.410335,12.369353,1959.341036,-8789.968366,18176.031111,-21228.034894,10779.468992,8250.834025,-20609.224850,14712.910998,5000.912523,-22268.932175,25004.319692,-15842.902093,5610.391090,-829.009529
+-3.716716,280.704489,-355.903224,-42.054028,411.441751,48.519658,-966.642939,1056.796821,306.389270,-1784.185307,1405.545188,941.197024,-3024.804943,2941.961264,-1386.139276,194.633758
+-12.998982,-54.523260,-107.431879,894.728680,-1811.407856,1537.414454,269.917037,-1799.743712,1168.710656,907.451000,-1775.945580,467.630285,1230.793798,-1536.221303,816.460944,-188.715147
@@ -0,0 +1,3 @@
+26.364149,177.069561,1575.881647,-9015.105328,20284.748852,-24877.180293,13157.069666,9693.742113,-24630.776658,17017.249779,6929.891099,-26402.315476,27658.672872,-15960.791210,4816.324261,-481.249223
+-2.697646,1243.905991,-6235.286985,17263.905468,-30739.116331,35075.762820,-19491.224818,-11225.334388,34615.605572,-28943.673202,-4147.157668,38977.054051,-50259.872605,36240.469382,-15200.785366,2862.042425
+-14.014894,-1092.010145,5356.556949,-12997.656894,19726.458704,-18688.134723,7112.080636,8917.358306,-17613.793090,11582.620986,4857.978470,-19032.575481,21707.141219,-14570.913691,5928.978876,-1176.673154
@@ -0,0 +1,3 @@
+27.488500,656.714564,-711.172967,-3607.344213,12064.577564,-16596.522652,8958.423854,7053.652016,-16703.379958,9887.716036,6672.954466,-17214.521473,14463.030066,-5503.534726,-56.863972,586.594485
+-1.853833,2825.199512,-14819.713309,39749.522052,-66654.957686,70216.103407,-33192.369894,-27929.172351,67590.964130,-50210.497342,-13479.819481,74116.096587,-90020.883283,62825.979648,-25821.438511,4852.075439
+-15.520495,202.519559,-1992.254633,6620.692936,-11609.000166,11322.598007,-3304.062495,-6929.925323,10421.235612,-4187.407718,-5464.971653,10171.718394,-7863.904914,3026.319376,-146.513897,-237.651350
@@ -0,0 +1,3 @@
+28.743402,2078.362604,-7793.565123,13384.615828,-12830.619300,5492.366699,2331.149832,-5092.810031,3520.435807,-1488.401128,-400.152676,3956.617004,-8314.481482,9383.643006,-5997.567239,1730.742485
+-1.213005,2555.978690,-13236.079583,35779.247030,-61178.493015,66475.594832,-33888.467431,-23956.790239,64629.202523,-51159.544433,-10075.409300,71917.010022,-90613.598094,64688.363705,-27025.918149,5135.121115
+-17.411436,702.346154,-4990.389270,14886.524601,-24997.074775,24043.706420,-7220.267390,-14427.214360,22387.881697,-9673.656090,-11279.697705,22289.916422,-17857.848174,7289.262350,-761.324352,-358.570553
@@ -0,0 +1,3 @@
+25.807032,-2925.872531,17626.900070,-48527.605690,78596.032935,-75357.034383,25236.004181,41036.456675,-70062.204933,37296.741902,27673.478531,-72007.179628,70351.596401,-40377.817337,13192.714073,-1823.721759
+-4.269878,3512.386346,-18619.333231,49177.358335,-80294.515135,81470.119172,-35154.890428,-35584.523231,77472.889766,-53740.991735,-18680.370211,83663.230433,-98251.161352,67389.126785,-27495.736176,5196.375807
+-8.223582,1274.911396,-6466.041128,15689.522036,-23322.244799,21493.532705,-8013.896644,-9664.522910,19462.409749,-14105.767315,-3182.501226,21122.648162,-28453.328443,22442.344490,-10549.264290,2316.605428
@@ -0,0 +1,3 @@
+28.265176,-979.386702,7419.686600,-22583.464563,37939.387710,-35793.651208,9323.655186,23112.091262,-32987.349099,11844.627678,18895.507035,-32027.986782,22354.058112,-6353.378068,-1374.824508,1159.236562
+-2.753809,5740.750259,-30974.191410,82232.589509,-134269.093565,135707.092980,-57790.468823,-59997.024412,128740.514055,-88400.008749,-31810.351026,138751.390069,-162076.229407,110706.908019,-44985.254781,8497.949168
+-10.288734,788.619135,-4015.272551,9464.238302,-13149.796146,10620.242441,-2203.465112,-6358.643341,9115.441480,-4803.765988,-3024.660611,9465.572049,-11175.374802,8141.654632,-3502.987926,673.165984
@@ -0,0 +1,3 @@
+30.325031,998.588079,-2506.097464,1112.679847,4050.362173,-7575.827035,3610.703835,4807.328728,-7912.810595,1423.062563,6908.855151,-7047.123610,-733.895950,6947.392420,-6118.106380,1990.418874
+-1.862138,5260.310326,-28362.138836,75966.525157,-125941.228823,130403.059750,-59433.171446,-53682.762909,124674.457655,-90703.860259,-26247.739993,136080.693972,-164317.109412,114677.998992,-47345.783502,9043.380773
+-12.328782,1149.268543,-6165.422014,15011.048121,-20807.425742,15269.068476,396.019698,-13600.991138,12819.784834,-209.559789,-10963.031285,11285.062493,-3827.872287,-2235.123927,2898.333874,-992.707506
@@ -0,0 +1,3 @@
+31.527635,1914.680208,-7066.786700,12001.498543,-11639.251141,5661.716637,833.889449,-3872.102019,4050.195339,-3333.621620,780.586620,4854.753625,-10852.776692,11992.859997,-7521.880502,2151.180312
+-1.186170,3479.031721,-18698.030760,51454.523473,-89011.366270,97857.359510,-51243.303570,-33721.655799,95275.494046,-77464.993449,-12838.707630,106615.132589,-136961.458686,99212.786606,-42070.122449,8177.263621
+-14.202558,-169.760541,386.222977,-846.701641,3119.384246,-7575.860335,9931.127792,-4154.570319,-8387.043291,16071.808162,-7926.249406,-12194.138559,27588.778783,-26471.170926,14053.983232,-3417.014125
@@ -0,0 +1,3 @@
+23.841122,-2194.779263,12904.935717,-34666.854475,54673.688646,-50635.353984,15102.364305,29120.803253,-46437.225110,22974.724498,19662.679126,-47233.232100,44960.041833,-25267.742184,7979.427992,-1008.602929
+-5.541790,2437.156126,-12346.835276,31462.020082,-49779.670442,48984.591505,-19933.267591,-22260.621101,46139.632315,-31280.871902,-11476.000089,49587.080568,-58457.462709,40644.366832,-16922.470009,3256.705963
+-6.078312,4452.065484,-24199.711547,63335.871292,-101626.656720,101030.434432,-42179.328607,-44663.085381,95065.016826,-66090.072687,-21936.472305,102617.427709,-123300.181672,86973.857880,-36758.005156,7353.148051
@@ -0,0 +1,3 @@
+28.618106,-288.792161,3141.199165,-10182.309050,16315.417146,-12565.923060,-1804.904848,13936.301857,-10686.677337,-4628.945671,14412.868226,-7786.295636,-7077.609479,14394.202010,-10151.845969,2932.493559
+-3.146467,6224.002286,-33674.042838,89395.509712,-145919.391650,147545.429134,-63071.762176,-64892.129879,139981.750282,-96642.262001,-34043.616251,151037.881495,-177225.657573,121541.053753,-49595.003797,9419.304769
+-8.139910,1042.248379,-5428.501831,13729.525415,-21836.979841,22646.227162,-11832.542811,-6780.352416,21416.920927,-19818.471600,130.693847,24841.636573,-37042.455599,29967.380524,-13973.035278,2985.845469
@@ -0,0 +1,3 @@
+30.810604,1307.872770,-4685.931426,8016.638132,-8868.736924,7291.247921,-4460.229391,-212.227482,6632.103103,-10276.374662,4770.665287,9048.021536,-21033.757954,21529.664708,-12359.641022,3262.133326
+-2.003713,5926.719259,-32038.394880,85604.852385,-141394.314399,145789.327084,-65936.879985,-60406.008704,139213.824099,-100916.225923,-29562.362503,151847.930645,-183232.805959,127952.503579,-52916.094043,10148.154329
+-9.920525,1375.816163,-7255.885038,17902.439980,-26219.206645,22434.826259,-5206.533249,-13942.467627,20123.256080,-9092.569591,-9086.668740,20409.257860,-19272.018210,10906.073918,-3480.786190,437.460015
@@ -0,0 +1,3 @@
+31.975666,1612.611465,-5710.058715,8983.152580,-7361.156073,1755.791235,2351.559612,-2104.864685,317.859148,-653.395208,1629.681386,644.939260,-5839.181279,8640.160027,-6266.314301,1950.947453
+-1.550916,5009.240516,-26964.424008,72614.247722,-121816.149633,128860.007481,-62472.103628,-49000.676378,124022.725423,-95624.567987,-21136.606762,137214.809484,-171792.694479,122820.585593,-51702.096671,10044.368612
+-11.512307,26.148037,-286.079982,260.064174,1800.601902,-6094.047095,8527.457143,-3801.084297,-6814.632040,13419.917602,-6774.440351,-9934.632608,22788.215433,-21918.884601,11642.657247,-2831.370660
@@ -0,0 +1,3 @@
+26.197525,-437.694497,3176.392508,-8582.550287,11493.328808,-5761.250067,-5973.267536,11639.945217,-3699.985563,-9689.670713,12511.953326,-272.462659,-14745.197405,18589.305875,-11322.773440,3035.082897
+-4.312181,5382.750782,-28956.350536,76477.012326,-124302.299392,125306.448999,-53475.187432,-54956.732272,118708.057825,-82386.303956,-28234.161379,128172.934300,-151638.507587,104956.992453,-43302.356871,8327.542935
+-6.237767,3407.862003,-18668.402438,49496.114618,-81134.142058,83758.382323,-39258.050462,-32407.212901,79742.732300,-61609.230412,-12768.578095,88282.327808,-112834.619373,82498.944583,-35641.096688,7206.281828
@@ -0,0 +1,3 @@
+30.257964,583.830210,-1208.136762,-2.526959,2307.048704,-1826.152951,-2722.543133,5796.497598,-1519.328038,-6933.762924,8772.887269,847.683336,-13639.685047,17503.530819,-11083.224620,3083.566374
+-2.528112,6221.602720,-33807.095242,90470.864195,-149454.640693,153985.111287,-69411.581584,-64092.936762,147001.161372,-106122.201244,-31627.909356,160149.763590,-192729.186294,134384.450096,-55553.298745,10670.421890
+-7.861770,2108.565894,-11258.246108,28746.526750,-44780.195243,42947.213456,-16524.082758,-20072.479055,39946.152469,-26746.123591,-9972.380017,42961.319178,-51085.084241,35820.355063,-14965.417848,2921.278175
@@ -0,0 +1,3 @@
+31.744656,1534.523358,-5660.106640,9645.556878,-9481.323315,4899.638607,124.608471,-2681.289259,3453.268987,-3692.985628,1667.287674,4202.151013,-10916.439306,12600.541302,-8088.121086,2347.044083
+-1.790821,5649.091225,-30477.524411,81634.880739,-135764.873445,141910.195977,-66938.534500,-55774.428590,136111.400298,-102746.145147,-25136.060368,149881.851485,-185513.231742,131780.189555,-55262.445023,10728.169679
+-9.274054,1126.681500,-6140.164082,15578.187652,-23326.557093,20201.799894,-4413.227212,-13265.477828,18310.881555,-6984.991725,-9777.115374,18115.716168,-14520.542549,6295.746276,-1029.613190,-144.927788
@@ -0,0 +1,3 @@
+32.313506,1584.092324,-5605.223993,8686.890411,-6523.413622,-2.202002,4551.203533,-2989.327460,-1631.950770,3005.277452,-68.230017,-2288.821021,379.995442,3006.308255,-3509.060531,1350.213936
+-1.506337,5345.528013,-28375.292418,75104.162089,-123727.629750,128389.113566,-60241.407731,-50241.425284,122781.994923,-93542.027541,-21546.018222,135630.097151,-169909.998876,121905.256717,-51560.297189,10067.545975
+-10.460958,-1316.085987,6799.771231,-17445.271239,28278.564054,-29478.079693,14794.815126,9955.022320,-27932.830735,23885.694804,1863.570759,-31465.729600,44324.464288,-35103.544734,16533.867367,-3693.047661
@@ -0,0 +1,3 @@
+27.951832,638.328802,-2351.931860,4975.986695,-8596.067151,12018.262162,-11023.162232,1571.306001,12384.143733,-18278.506780,6593.108603,16287.247678,-32348.383393,30172.504130,-16062.609657,3987.108885
+-3.365734,6018.273497,-32517.973622,86211.183101,-140724.620243,142658.110939,-61742.681870,-61802.247860,135455.457804,-94888.078792,-31579.184826,146601.962661,-173914.026220,120363.800930,-49560.596992,9502.418884
+-6.104348,3201.275314,-17754.562836,47695.895661,-79329.702458,83416.393064,-40758.508779,-30635.945148,79769.551898,-63605.472743,-10946.521484,88831.098618,-115702.271403,85680.661552,-37444.718056,7658.547740
@@ -0,0 +1,3 @@
+31.001824,930.719531,-2793.108593,3141.476068,-661.183171,-1864.663995,764.308655,2442.430751,-2442.139019,-2166.699457,5355.493755,-1429.245593,-6839.577772,10965.631673,-7804.379965,2355.559021
+-2.219340,5936.165707,-32321.742502,86962.840562,-144877.866358,151271.084273,-70695.329596,-60394.820305,145028.774887,-107983.603836,-28250.329929,159080.962287,-194920.224629,137529.914036,-57399.015738,11118.445993
+-7.431729,2898.169478,-15788.839688,41074.656624,-64932.563599,62736.861890,-23724.908095,-30398.636526,58608.021645,-37210.526407,-16967.686410,62273.922682,-70282.054004,47062.879008,-18838.177823,3539.121855
@@ -0,0 +1,3 @@
+32.040952,1616.150391,-6006.612591,10103.834150,-9091.665921,2665.776624,3395.883844,-4060.733768,762.016598,1191.746895,-390.783941,298.603416,-3136.929357,5769.256275,-4802.103048,1634.597926
+-1.731759,5375.646507,-28720.205073,76389.339010,-126524.838131,132252.829871,-63093.298986,-50836.460308,126848.228539,-97650.463985,-21480.671839,140429.500638,-176591.235969,126919.183230,-53763.216414,10527.978609
+-8.566198,1112.969579,-6329.117843,16823.318957,-26605.949524,24911.270891,-7640.792073,-14419.877024,23218.405056,-11214.921311,-10412.068216,23593.670297,-21189.234632,10816.082774,-2832.909204,195.963399
@@ -0,0 +1,3 @@
+29.031243,1024.647321,-4033.114979,8135.361831,-11329.985559,11613.567786,-7429.836583,-1456.944928,11074.724697,-13576.678971,3659.856096,13706.996434,-25848.280416,24274.757660,-13239.575621,3387.966126
+-2.869995,6307.197708,-34222.898443,91148.821264,-149598.974883,152769.155060,-67355.864507,-65020.079706,145410.184862,-103247.130664,-32732.918444,157819.533683,-188440.442327,130895.541062,-54031.350214,10383.858405
+-5.874280,3482.595948,-19408.919657,52253.155752,-86779.602070,90621.011001,-43171.320274,-34612.088869,86472.749651,-67050.484129,-13692.676843,95597.036137,-122197.691754,89465.504669,-38797.665513,7896.161794
@@ -0,0 +1,3 @@
+31.364619,1212.655810,-4140.134031,5989.654081,-3741.079063,-1040.246094,3086.315995,-540.557386,-2345.872467,1007.627429,2465.798756,-2341.000707,-2489.858931,6474.019714,-5471.225438,1825.025631
+-2.085064,5342.386691,-29058.805335,78508.767204,-131865.271205,139602.464184,-67736.448951,-53113.804084,134433.995467,-103508.997051,-23081.781604,148606.763393,-185796.096666,132816.415605,-56006.731815,10940.088391
+-6.979470,3531.315531,-19481.141875,51319.072579,-82101.614578,80259.965487,-30994.103031,-38573.364768,75284.207028,-47868.363905,-21961.493090,79934.241557,-89537.598855,59416.260125,-23569.998180,4397.467051
@@ -0,0 +1,3 @@
+32.118330,1644.395389,-6254.807567,10873.889952,-10278.101757,3446.898034,3808.518260,-5268.521377,1341.247037,2017.006948,-1677.936611,477.703238,-1874.368294,4159.726362,-3880.770230,1411.729386
+-1.799531,5298.851513,-28031.782953,73822.225340,-121146.249448,125568.067255,-59252.700865,-48553.198324,120116.072837,-92517.590338,-20070.157422,133100.603297,-168171.510185,121441.114109,-51685.750957,10163.900045
+-7.948920,1165.252117,-6907.991430,19221.711094,-32063.063965,32330.929643,-12724.446375,-15983.772200,30791.208957,-18345.762287,-10642.076057,32259.043449,-33020.319993,19673.032543,-6721.993394,998.677553
@@ -0,0 +1,3 @@
+18.957378,-2142.429781,13031.460439,-37132.714349,63917.152425,-68221.643247,32874.254900,26378.039108,-65510.622479,49567.820526,12099.520876,-72095.453763,88791.645495,-62475.499863,25722.781741,-4881.104252
+-7.271660,1230.781638,-5139.998814,10339.745576,-11638.325252,5789.985878,3089.137797,-6940.535159,3366.473968,2062.320239,-3598.192612,2118.637907,-1494.153688,2090.527628,-1728.767106,498.555412
+-4.250695,1145.198970,-6696.814130,19147.050318,-34175.335883,38988.235400,-21961.874824,-11949.438784,38371.594238,-32947.115639,-3594.386974,43405.612014,-57564.387475,42600.989204,-18463.475468,3729.467912
@@ -0,0 +1,3 @@
+29.634646,874.371880,-2924.947771,4392.655364,-3802.310823,2203.644208,-1516.694047,945.856883,1621.237997,-5037.255479,4257.793478,2910.650092,-11412.712613,13644.807785,-8673.929571,2473.914105
+-2.624312,6195.977834,-33737.320255,90322.525995,-149256.129733,153900.699485,-69566.349436,-63842.519699,146951.923111,-106371.067267,-31316.979263,160138.916176,-193162.809545,135024.310391,-56004.325081,10808.417754
+-5.626598,4419.272666,-24466.289492,65143.740175,-106310.816029,107816.701255,-47221.328386,-45627.352725,102055.064601,-73304.532897,-21632.295075,110946.856485,-135117.034158,95825.423792,-40531.065915,8083.223359
@@ -0,0 +1,3 @@
+31.596820,1491.996250,-5598.607060,9502.469992,-8591.297693,2494.458195,3211.437315,-3694.993149,561.108482,987.222006,25.640706,150.838015,-3462.676894,6277.742491,-5122.538723,1717.779398
+-2.124735,4997.617817,-27063.772146,73055.518237,-122967.334431,130980.693732,-64818.203195,-48381.322124,126393.156404,-99332.249036,-19814.871788,140442.571604,-177948.522841,128324.002277,-54486.257170,10700.553053
+-6.569346,4356.732743,-24252.920685,64500.521465,-104215.385706,103048.585056,-40905.184137,-48600.836606,96987.980148,-62644.487211,-27517.529599,103210.932228,-116304.060894,77446.890145,-30830.849244,5782.926212
@@ -0,0 +1,3 @@
+32.237271,1576.979840,-6011.105218,10537.182412,-10168.827661,3713.062074,3489.757595,-5306.308416,1661.733853,1842.791653,-1826.392065,715.856460,-1979.616956,4169.964558,-3893.728187,1424.641377
+-1.875380,5974.190884,-31519.799452,82324.293177,-133499.684063,136087.475561,-61770.108925,-54872.448189,129448.464695,-97116.471394,-23796.214851,142650.336726,-178069.672198,127804.236988,-54197.153437,10639.750686
+-7.428003,730.853985,-4892.614938,15085.233982,-27716.928753,31175.421498,-15762.115417,-12146.514696,30612.353263,-22182.982912,-7113.943754,33191.149366,-38025.284272,24959.950814,-9545.961325,1664.630000
@@ -0,0 +1,3 @@
+19.582381,-1944.597644,11704.912267,-32905.095208,55666.529915,-57953.486980,26135.611847,24346.629133,-55387.829992,39369.638042,12757.567607,-60308.681675,70879.985327,-47846.952730,18738.520936,-3318.620647
+-7.210948,3484.292663,-17786.198177,44325.090196,-67302.646391,62005.807840,-20790.055633,-31771.140653,56582.739056,-34555.398716,-16626.018559,59510.043449,-68754.265121,48175.015263,-20542.262738,4113.275553
+-4.090756,1078.592263,-6853.831501,21247.065828,-41011.193251,50798.298513,-32791.791165,-11375.156667,50962.408894,-48860.037271,-187.417001,59257.424304,-83720.105154,64126.940269,-28504.128333,5880.916978
@@ -0,0 +1,3 @@
+30.046116,796.992567,-2314.903793,2220.454766,773.119258,-3785.342421,2524.157949,2194.054117,-4465.822804,805.804648,4331.776158,-4152.042288,-1615.416982,6274.791566,-5455.785712,1820.631724
+-2.498103,5950.357153,-32513.296291,87545.026280,-145769.831516,151925.342550,-70537.031442,-61208.952051,145567.295506,-107588.765697,-29085.258272,159336.522346,-194347.581963,136788.859812,-57032.763200,11055.250318
+-5.387076,5458.089371,-30190.809038,80107.506752,-129773.642153,129715.074618,-54095.133492,-57923.523629,122305.139097,-83741.048054,-29870.725816,131639.878808,-155230.532238,107515.679598,-44564.229048,8732.065402
@@ -0,0 +1,3 @@
+31.823995,1724.762800,-6899.334458,12917.520571,-13923.760460,7411.117717,1717.695803,-6435.202319,5004.251814,-1360.782522,-1618.005310,4654.184094,-8218.944316,9425.938085,-6437.751007,1991.297469
+-2.307275,5251.485843,-28273.224797,75757.169185,-126545.892420,133748.478898,-65368.644097,-50000.938124,128784.096000,-100724.491328,-20520.930509,143042.022303,-181105.569681,130623.859030,-55484.617786,10901.523977
+-6.218630,4764.538869,-26802.648562,72111.374802,-118055.054424,118769.901432,-49421.422435,-53823.559668,112312.081428,-75210.054182,-29449.411514,120254.236410,-138378.704667,93647.618983,-37870.025482,7223.902442
@@ -0,0 +1,3 @@
+32.373059,1112.776820,-3583.384825,4502.467941,-1120.374610,-4442.300108,6007.899471,-880.018083,-5808.227076,6086.192488,778.614495,-7145.339477,6410.937974,-1170.787572,-1878.360323,1072.512212
+-1.881201,6854.955461,-36237.630023,94329.376769,-151873.475174,152922.239142,-67075.012540,-63981.943668,144823.592371,-105758.002193,-29158.346943,158634.391627,-195203.481989,138944.386296,-58597.097271,11470.536911
+-6.977581,-14.400007,-1253.041679,6981.692594,-17717.429546,25588.417962,-18498.644453,-4716.944510,26568.663948,-25459.917227,-680.811151,30582.088256,-41320.032399,30354.625255,-12902.245552,2525.805041
@@ -0,0 +1,3 @@
+22.520164,-1811.124454,11047.564051,-31078.856822,52208.044994,-53692.790893,23588.424060,22928.235432,-50924.457178,35937.061955,11728.633205,-55383.831413,65491.259210,-44534.360334,17507.142754,-3082.442677
+-5.468960,5753.281237,-31125.716910,81971.394700,-132003.413167,130785.412020,-53117.719638,-59595.284508,122593.437229,-82570.475078,-30663.882956,131196.284423,-154737.749547,107965.356424,-45293.980900,8934.326936
+-4.071809,3941.881452,-22391.523294,61684.168694,-105361.499053,114244.520883,-59176.523188,-39060.518027,110325.468350,-91010.165273,-12745.982558,123628.665961,-163110.350583,121569.230670,-53487.801764,11045.494527
@@ -0,0 +1,3 @@
+31.894480,704.302861,-1650.323389,181.114795,4284.804143,-7143.414631,3259.352761,4726.296999,-7740.742400,1408.397582,6993.837431,-7270.085292,-896.282956,7811.733728,-7088.483823,2376.967479
+-2.458160,9352.278411,-51702.738405,139446.245712,-231286.255467,238644.109332,-107277.616701,-100023.725978,227883.955953,-163177.929551,-50456.574214,247666.483368,-295887.225995,205188.714228,-84495.899747,16242.390607
+-5.236569,7036.413796,-38919.471734,103221.194390,-167046.694647,166697.036561,-69293.688071,-74489.295537,156941.485080,-107533.126718,-38064.492526,168904.531569,-199833.982898,138945.303175,-57856.113430,11396.448043
@@ -0,0 +1,3 @@
+33.277677,1283.544707,-4069.219315,4425.981255,1415.709806,-9728.663876,10528.086946,-74.610764,-11806.972395,11414.549293,1723.083448,-13901.436372,13818.681313,-5341.750559,-738.295349,998.677180
+-1.700823,5867.892823,-31934.775528,86390.818547,-145711.534661,155720.225688,-77801.497702,-56690.889130,150433.501225,-119342.504238,-22582.878831,167574.003155,-213491.597002,154459.315006,-65753.632202,12957.253670
+-5.873377,5452.957877,-30867.873073,83638.682498,-138072.817151,140559.954500,-60604.777739,-61302.633030,133063.881726,-92324.425168,-31529.495548,143238.741258,-169622.512264,117828.596087,-49010.431298,9646.259405
@@ -0,0 +1,3 @@
+26.858760,-434.443303,4800.482829,-16959.521527,31758.356920,-34173.881003,14342.720848,16577.699462,-32769.993134,19112.137370,12080.635018,-34081.032376,32924.202451,-17755.570415,4895.961784,-375.664523
+-2.723993,2720.803509,-14296.331585,38236.327851,-63749.085291,66566.734471,-30770.807737,-27175.745144,63902.250648,-46589.300871,-13529.730307,69777.769952,-83845.351861,58124.379941,-23777.224729,4453.363811
+-14.455692,-1335.706870,6854.576649,-17287.531514,27050.526239,-26255.638215,10310.908590,12393.231476,-24809.415810,16228.159909,6953.984690,-26633.765119,30086.914470,-20043.637608,8091.953105,-1589.091494
@@ -0,0 +1,3 @@
+28.181865,249.836154,1295.756263,-8080.546419,17512.238816,-19428.174699,7076.301707,11316.736924,-18608.042301,7489.148773,10436.823339,-18146.253050,11751.767902,-1818.194899,-2226.777613,1130.603497
+-1.757510,4609.852730,-24556.201479,65016.922841,-106181.127795,107430.746282,-45827.369923,-47477.674747,101996.839278,-70026.782106,-25293.364764,109987.170146,-128212.085127,87259.498380,-35235.726260,6568.643636
+-15.964545,7.467719,-654.911308,2319.778755,-3269.809312,1251.909774,2654.393606,-3941.055571,348.849030,4351.766758,-4282.560934,-1133.211771,6581.334724,-7341.104609,4253.444452,-1121.194589
@@ -0,0 +1,3 @@
+29.567700,1865.036949,-6787.807747,11431.698529,-11303.509841,6440.922536,-1015.354910,-2624.677863,5177.554165,-6210.696674,2369.741660,6849.449777,-15366.855759,15968.108629,-9323.300777,2492.576312
+-1.040541,2946.947725,-15690.292889,43096.409876,-74346.187369,81051.872174,-41142.507220,-29625.500516,78804.919856,-61572.158177,-13109.806317,87304.023396,-108808.798515,77089.165399,-32021.603931,6072.662537
+-17.836569,92.385342,-1497.414704,5317.610570,-9009.441875,7498.041622,215.657320,-7463.494290,6581.402206,1566.062062,-7688.179813,5165.651799,2458.686529,-6772.856395,5074.165896,-1512.041784
@@ -0,0 +1,3 @@
+26.390744,-2362.866410,14234.543664,-38687.131652,61077.022666,-55759.262457,14786.609539,34419.140449,-50985.699248,21712.517766,25209.973678,-50766.486213,42884.773678,-20209.575730,4386.943338,-0.828732
+-4.231894,5712.113545,-30636.197509,80470.717531,-129594.961837,128556.556034,-52101.411012,-59311.883104,121227.045053,-80286.771794,-32468.432413,129714.867945,-148848.582934,100620.379206,-40628.115666,7652.001334
+-8.647257,714.757880,-3457.831489,8134.454947,-12216.503616,12371.761072,-6866.992177,-2790.191271,11483.535570,-12340.115496,2096.481887,13848.351726,-23823.925591,20960.897693,-10417.794624,2346.506002
@@ -0,0 +1,3 @@
+29.451271,440.117958,-605.785372,-721.528759,1440.985626,2072.068807,-7881.908289,7431.707784,3153.742344,-14253.089213,11164.058079,7214.589481,-24975.123803,26900.498108,-15383.301878,3968.691215
+-2.594677,7050.985034,-38088.366052,100689.363504,-163251.716510,163266.060630,-67554.612778,-74047.589816,154324.894914,-103715.829237,-40074.214507,165640.126419,-191344.614132,129722.106853,-52373.497680,9839.300441
+-10.742429,340.165488,-1545.261204,3060.484230,-3299.405059,1804.716913,8.990599,-847.717386,1149.990634,-1468.698829,823.824469,1683.605434,-4254.491883,4296.970437,-2145.000126,424.594389
@@ -0,0 +1,3 @@
+31.479088,2051.557654,-8479.521989,17532.892421,-23764.966977,21923.800376,-10551.792544,-6681.911514,20478.754265,-19967.810634,1575.400218,24089.672876,-39051.858523,34154.596223,-17653.429778,4313.663748
+-1.651955,6026.531620,-32767.707429,88146.352660,-146462.352614,151713.032123,-68896.507464,-62881.180071,145041.747197,-104789.629224,-31286.047692,158017.004257,-189711.023347,131788.962656,-54168.392711,10306.635333
+-12.734173,747.836787,-3848.402992,8611.306044,-10010.129494,3922.993369,5755.588995,-9170.359494,2014.724744,7972.803966,-9087.394361,-515.906521,11087.495932,-13176.704725,7722.420024,-2006.935217
@@ -0,0 +1,3 @@
+32.916094,2791.116480,-11752.909409,24107.370470,-30822.701332,24422.031448,-6642.289612,-12526.174399,21626.289005,-15099.785474,-3683.690787,23659.743799,-32988.901798,27483.272426,-14111.380146,3501.396152
+-1.081277,3522.495769,-19187.418598,53645.309597,-94403.900918,105831.799562,-57450.259971,-34623.200253,103666.705235,-86355.969715,-12287.599090,116671.120484,-151440.757039,110156.397652,-46770.132874,9088.613687
+-14.566224,-639.353764,3023.123394,-7946.245456,14826.918093,-19643.773641,15537.709509,514.860327,-19770.855492,24891.253804,-6234.641470,-24717.868049,43947.940118,-38784.992729,19593.815162,-4597.859528
@@ -0,0 +1,3 @@
+24.773492,-1264.695209,7587.757276,-20034.101951,29995.252976,-24706.054955,2918.028229,18862.026292,-21707.111780,4470.389864,15110.633467,-20290.412453,11322.736811,-819.902773,-2731.995112,1237.800469
+-5.511904,3964.801529,-20902.527320,54475.512923,-87619.773205,87558.000731,-36919.055238,-38544.107336,82668.385977,-57504.959973,-19256.454577,89281.660183,-106684.222452,74727.202371,-31253.202576,6081.055472
+-6.614628,3958.344894,-21527.145950,56593.659538,-91757.257167,93172.776018,-41760.367097,-38053.208328,88314.550294,-65664.290179,-16524.013637,96960.935475,-121071.558038,87221.488445,-37256.901272,7465.784879
@@ -0,0 +1,3 @@
+29.841135,1371.745857,-6086.855568,14451.924219,-23833.338865,27773.304027,-18719.089132,-4093.774446,27412.976321,-30575.232929,4934.440400,33092.274281,-54989.366628,47512.349031,-23958.467531,5684.456484
+-2.964036,7728.604291,-41956.775350,111204.823180,-180751.979500,181426.763445,-75898.975317,-81439.186860,171687.202569,-116460.400494,-43606.062437,184608.466878,-214435.005390,145975.381082,-59160.171042,11167.307176
+-8.543289,1749.370137,-9246.812592,23557.178927,-37104.515347,36883.379243,-16432.775921,-14610.721101,34675.463419,-26942.821029,-5082.185147,38615.590326,-50571.498858,37716.961721,-16487.873597,3331.459711
@@ -0,0 +1,3 @@
+32.160869,2191.479660,-9458.724309,20447.455743,-28697.937442,26811.237799,-12362.487808,-9092.316231,24926.759588,-22668.338348,277.663463,28638.286964,-44310.072866,37963.363478,-19418.465171,4723.002623
+-1.859483,7358.832923,-40105.721767,107437.460678,-177438.152256,182475.294048,-81659.014832,-76651.557175,174104.763324,-124706.990857,-38450.375181,189406.806522,-226549.023873,157101.808323,-64545.084252,12311.798579
+-10.256590,1405.744655,-7178.104567,16956.856943,-23221.904511,17318.495092,-504.032972,-14293.484522,14777.393075,-2080.197940,-10911.588114,13677.295260,-7455.913464,714.172835,1511.772591,-690.762715
@@ -0,0 +1,3 @@
+33.352905,2417.788822,-9991.532638,19980.183101,-24681.737899,18573.718659,-4243.485479,-9938.941078,16025.247514,-11100.310233,-2386.264378,17414.644136,-25619.337609,22571.361701,-12260.115710,3198.546374
+-1.380778,5476.216510,-29835.669193,81189.169342,-137565.157003,147054.885284,-72640.254647,-54857.721926,142023.431645,-110509.299351,-23526.957263,157417.189335,-197397.907753,140994.501316,-59211.961438,11470.407813
+-11.754313,-677.906363,3673.656283,-10428.721602,19480.171375,-24353.988333,16963.796372,3419.760290,-24137.475640,26546.363040,-3794.237453,-28946.703224,46894.857531,-39576.925381,19369.227446,-4433.651118
@@ -0,0 +1,3 @@
+27.660583,1988.451694,-10129.551297,26396.019684,-44468.217317,49024.092007,-27298.463665,-14448.185330,47632.373853,-42683.727963,-1995.431388,54202.695185,-76613.712040,60451.961543,-28484.873877,6411.405274
+-3.894782,6911.988271,-37333.916161,98443.944295,-159247.524166,159158.743729,-66204.256101,-71506.170268,150317.798801,-102189.788993,-37671.365405,161669.794494,-188988.379221,129641.711271,-53028.677074,10114.312850
+-6.616320,4367.998914,-24088.525848,64173.560530,-105419.214112,108650.328933,-50237.535144,-43073.072876,103514.090092,-78325.513693,-18415.271285,114115.087158,-143051.524020,102966.293131,-43835.469600,8740.310388
@@ -0,0 +1,3 @@
+31.668587,1795.200598,-7770.499283,17079.626500,-24814.727365,24572.045644,-12945.558457,-6741.863178,23153.262371,-22914.812955,1991.824129,27049.754483,-43796.558877,38252.245825,-19781.555735,4843.187949
+-2.281542,8075.159241,-44203.389959,118407.882093,-195106.934747,199713.266101,-88112.034803,-85216.710870,190223.940058,-134502.519122,-43560.218583,206292.674453,-244949.153587,169125.086473,-69309.435608,13219.988866
+-8.145047,2696.068051,-14332.670965,36321.797191,-55794.599745,52004.377157,-17812.816251,-26753.779936,48037.074905,-28850.661403,-15241.374715,50679.390361,-55946.345934,36893.683404,-14510.235185,2655.657664
@@ -0,0 +1,3 @@
+33.095976,2152.118291,-8881.755325,17751.571206,-21973.379351,16738.927075,-4299.582060,-8310.200116,14388.013059,-10919.114624,-1069.093949,15844.362600,-24922.232859,22749.322552,-12617.267502,3329.413601
+-1.629301,6984.736243,-38146.290727,102836.227371,-171631.368374,179581.004249,-84350.652121,-71284.704488,172311.543150,-128810.440733,-33195.123641,189271.329181,-232165.944474,163704.178487,-68171.348375,13160.821925
+-9.465573,1207.103526,-6365.911728,15489.522852,-21766.752423,16520.631023,-248.592411,-14336.848681,14291.935234,-754.603439,-12029.877956,12745.288860,-4192.816314,-2897.294148,3544.887251,-1186.993664
@@ -0,0 +1,3 @@
+29.535354,1990.789675,-9611.914868,23689.011726,-37984.300226,40224.977052,-21547.832032,-12153.302911,38597.878487,-34908.842791,-846.172275,43999.328886,-64117.264158,52084.114215,-25314.316062,5878.657243
+-3.018719,8237.316078,-44852.238234,119038.853335,-193792.652711,195080.814063,-82425.180460,-86673.371989,184760.416590,-126530.880033,-45771.753526,199024.999272,-232698.983942,159277.487579,-64921.251600,12343.393826
+-6.387072,3872.411459,-21421.379848,57225.800079,-94259.625809,97477.710808,-45477.206924,-38151.871562,92834.776181,-70914.953769,-15732.902324,102427.701335,-129632.304199,94160.493392,-40490.566693,8161.518697
@@ -0,0 +1,3 @@
+32.416195,1664.417162,-6682.586866,13086.999481,-16238.804669,13163.468616,-5059.079769,-4584.252374,11541.821541,-11469.122878,1832.660716,13478.359483,-24586.507337,23634.967132,-13356.031761,3536.474783
+-1.969092,7918.506253,-43586.322535,117660.895553,-195838.479688,203360.087317,-93116.513000,-83440.877776,194594.514858,-141711.038804,-40944.196152,212339.959086,-256226.501404,178774.853863,-73891.594226,14207.855408
+-7.608015,3365.666296,-18129.232647,46498.368984,-72090.482053,67474.383528,-22755.175910,-35517.804074,62445.429029,-36025.467924,-21442.815150,65283.009273,-69480.750893,44280.852728,-16849.071396,2990.151175
@@ -0,0 +1,3 @@
+33.215169,1918.728026,-7576.090530,14033.650467,-15157.855228,8539.840378,901.341859,-6438.676318,6180.442737,-3017.235031,-1082.711648,6260.352706,-11281.137514,12227.384270,-7899.724458,2345.460074
+-1.602410,6196.486859,-33650.456257,90637.517917,-151749.615571,160153.955527,-77387.462552,-61103.616268,154108.755297,-118624.064353,-26467.224332,170480.157713,-213199.040922,152339.447259,-64143.843601,12499.454209
+-8.662570,1542.642106,-8450.407603,21634.359133,-32694.038740,28447.920640,-6025.063788,-19087.067337,25792.116172,-9101.201770,-14506.375832,25151.744389,-18980.132282,7375.128886,-744.816890,-330.717893
@@ -0,0 +1,3 @@
+19.365294,-2095.545193,12992.367455,-37467.667008,65124.303637,-70421.563197,35300.578276,25407.464257,-67481.301403,53716.162580,9491.333807,-74885.706094,96742.562399,-70965.793749,30557.567376,-6105.052372
+-5.583529,-2486.727618,15256.869231,-43173.947058,73908.593900,-78341.598347,36872.854698,31784.537550,-75723.048351,54958.401592,16993.619680,-82913.887366,97087.985081,-65023.217137,25530.304408,-4725.430282
+-4.624419,2033.915685,-11388.386977,30975.695527,-52403.626425,56455.140312,-29035.429889,-19497.573995,54570.088323,-44823.940879,-6557.671939,61205.294352,-80336.897382,59525.776427,-25977.801599,5306.367769
@@ -0,0 +1,3 @@
+30.503881,1637.998679,-7267.999690,16378.248790,-24223.472854,24081.439332,-12359.939876,-7130.949283,22646.884895,-21518.618415,1042.236825,26085.752419,-41081.765839,35497.556767,-18318.082781,4501.993109
+-2.571367,8697.612353,-47588.673985,126925.880254,-207796.398876,210723.488448,-90714.763504,-92073.086149,200061.627326,-138828.581520,-48035.845962,216060.950625,-254203.822609,174650.692703,-71395.106346,13618.449711
+-6.087855,4200.955011,-23038.088867,60786.801336,-98367.993355,98956.181772,-42778.775678,-42207.440561,93450.796283,-66974.818386,-19827.254609,101662.537188,-124045.408718,88122.512060,-37287.120290,7422.819137
@@ -0,0 +1,3 @@
+32.669731,1464.640323,-5418.195010,9167.366035,-8704.684489,3949.747897,687.515681,-2263.097535,2319.875738,-3005.414089,2240.864417,2895.811859,-10111.899780,12756.547030,-8582.235791,2559.256552
+-1.856224,7259.600381,-40068.198408,108820.895395,-182726.755646,192249.916662,-91039.232026,-75869.293774,184744.217173,-138314.909205,-35519.521132,202813.237921,-248586.080019,175207.222366,-73010.102773,14138.451462
+-7.072842,4082.259966,-22267.625644,57891.573803,-91085.034900,86842.232631,-30928.317428,-44268.990274,80792.822117,-48238.362354,-26346.450654,84843.567516,-91799.492134,59301.103604,-22928.121380,4163.244189
@@ -0,0 +1,3 @@
+33.206614,1853.327255,-7244.657524,13071.338938,-13167.761045,5668.764047,3326.456526,-6441.130986,3162.926365,741.584811,-1871.627411,2439.283272,-4941.604887,6964.482860,-5420.429639,1808.832444
+-1.604248,5730.800286,-30858.243925,82656.553652,-137988.359441,145702.695463,-71070.131302,-54579.121498,140230.285429,-109580.670268,-22410.380140,155754.869823,-197151.151646,142131.688018,-60311.313360,11830.168397
+-7.994372,2076.524949,-11708.916682,31135.178498,-49541.510260,46982.992611,-15364.347524,-25996.175738,43723.566810,-22829.733941,-17684.599938,44781.672320,-43283.996237,24696.949282,-8108.054201,1153.333265
@@ -0,0 +1,3 @@
+20.524085,-2384.106779,14334.064346,-40208.886730,67851.511272,-70551.417413,32067.724115,28987.895685,-67104.685871,48863.207647,13932.199634,-73287.353094,88806.778148,-61915.793992,25259.444872,-4730.313075
+-5.542702,1204.595984,-5391.281348,12181.374045,-16636.284882,13152.549297,-2315.375343,-8095.136310,10814.526678,-5457.890606,-3201.902999,10667.724450,-14147.136122,12189.494755,-6419.637380,1500.040893
+-4.434680,3483.180113,-19892.713623,55038.374000,-94311.913589,102443.264457,-52966.826967,-35393.614569,99147.858125,-81063.481820,-12451.808957,111017.493542,-144649.481616,106480.578384,-46188.857561,9386.334375
@@ -0,0 +1,3 @@
+31.095676,1259.940785,-4928.388381,9470.135848,-11799.203255,10143.058201,-5019.926187,-2231.953954,9028.611048,-10735.422885,3213.629875,10995.449259,-22257.016109,22164.661712,-12755.069239,3415.397633
+-2.347638,8846.783859,-48615.068766,130297.200740,-214541.717146,219226.621137,-96199.882411,-94086.196643,208653.699271,-146804.477482,-48387.557818,225950.297513,-267659.993646,184677.248038,-75750.288862,14499.495494
+-5.778510,5365.687135,-29405.391198,77306.277174,-124087.882410,122793.285592,-50158.609340,-55612.433236,115416.348491,-78336.501816,-28652.117521,124115.053152,-146151.462575,101240.198342,-41964.350729,8213.778006
@@ -0,0 +1,3 @@
+32.810843,1375.563071,-4834.722141,7261.430230,-4821.861813,-1095.720968,4120.642254,-1251.301197,-2832.641833,2006.411530,2249.645821,-3116.454886,-1708.361710,6405.151884,-5797.098406,1991.878370
+-1.851467,6773.136016,-37371.563432,101740.892017,-171627.601637,181984.590539,-88005.438525,-69906.367693,175328.437031,-133745.199262,-31466.981144,193312.155238,-239600.670353,170109.178367,-71303.372631,13876.453012
+-6.618097,4996.330215,-27500.930286,72209.783004,-114886.850514,111166.013744,-41417.786426,-54919.243522,103818.621659,-64107.655830,-31914.924294,109570.014652,-120901.914292,79426.732484,-31288.087687,5815.727935
@@ -0,0 +1,3 @@
+33.255618,1701.464062,-6447.746607,11003.506907,-9754.166709,1971.192604,5372.232929,-5363.806344,-449.823926,4023.157333,-1714.903796,-1767.275802,900.157622,2533.091470,-3475.086435,1414.773242
+-1.553495,6163.359174,-32962.081282,87383.445202,-144086.029837,149857.676526,-70887.898824,-58045.452082,143519.224266,-110111.249580,-24546.196196,158815.379963,-199638.013728,143556.265212,-60876.251876,11948.782786
+-7.433983,2354.334624,-13702.236763,37753.425454,-62667.458991,63134.035772,-25242.735416,-30351.698923,59782.006675,-37095.076748,-18769.239958,62917.328031,-67736.839907,43054.092373,-16231.798493,2845.048827
@@ -0,0 +1,3 @@
+21.514746,-1675.125871,10746.532882,-31438.946684,54789.432380,-58932.264009,28957.683829,21867.781711,-56293.822759,44008.486102,8707.502139,-62325.230163,79402.905758,-57415.569501,24187.395267,-4668.209959
+-3.910112,-4241.466210,23573.644364,-60773.415309,93591.390837,-85887.706636,26461.486785,48047.486393,-79318.439727,41902.993253,31474.227937,-82171.957140,81034.801182,-47113.529080,15951.442219,-2527.791017
+-4.323854,6695.030912,-37378.929779,100415.462678,-165683.567119,170861.633741,-78402.081698,-68716.204329,162742.180793,-121438.156621,-30657.234353,178577.312295,-221558.515627,158630.017478,-67513.185218,13539.603054
@@ -0,0 +1,3 @@
+31.517201,961.052003,-3143.302629,4352.202314,-2830.953757,359.618797,-160.045751,1513.796884,-484.724619,-3590.911681,5105.710992,560.704438,-9716.014506,13539.372702,-9262.998089,2756.143658
+-2.331790,9089.464607,-50144.649052,134949.160762,-223241.414167,229506.870703,-102217.014757,-97103.554359,218878.574608,-155633.172470,-49387.760301,237523.411268,-282790.235247,195700.337895,-80457.491676,15438.569240
+-5.495639,6247.525896,-34355.740577,90557.948458,-145564.199455,143990.025341,-58442.744397,-65747.440248,135294.093696,-90972.134096,-34438.444145,145149.921278,-169728.476697,116958.773058,-48284.330115,9425.969372
@@ -0,0 +1,3 @@
+32.994469,1354.579587,-4590.685623,6196.733993,-2242.256209,-4892.452743,7084.825055,-838.215964,-6839.614645,6276.798664,1977.934109,-7942.782874,5227.419594,1185.034709,-3562.939488,1556.224013
+-1.821043,6342.218791,-34864.389241,94869.515285,-160387.313083,171053.191660,-84258.901607,-63947.849094,165118.598564,-128390.770818,-27361.936486,182929.426635,-229572.596630,164346.572603,-69348.270295,13568.534906
+-6.228475,5494.544653,-30537.724211,81057.024034,-130608.071875,128618.988676,-50551.697763,-60927.879222,120657.369066,-77768.939883,-34044.579077,128217.635942,-145288.711502,97601.284095,-39345.342570,7504.582282
@@ -0,0 +1,3 @@
+33.435960,1242.401519,-3962.967895,4594.081826,304.082284,-7759.163303,9235.736110,-945.695840,-9547.109424,10289.866079,450.937755,-11663.435529,12705.986959,-5667.580234,-103.956956,768.103096
+-1.336397,6866.741970,-36418.959669,95299.472361,-154600.523127,157446.677375,-71119.787313,-63931.711665,149720.130428,-111650.241109,-28183.809414,164727.685613,-204750.413797,146545.575345,-62030.939198,12175.933958
+-6.939453,1623.556153,-10318.636245,30834.408278,-55508.188352,61576.593042,-30934.342031,-23531.853361,59858.460044,-44588.576919,-12106.782791,65112.779851,-78088.546924,54013.042978,-22147.962880,4257.630037
@@ -0,0 +1,3 @@
+24.755681,-3422.558154,20160.321190,-55625.123088,92327.980112,-93937.031702,40252.468918,41213.142321,-88996.876037,61420.476761,21825.044932,-96295.708153,112268.277236,-75787.490084,29874.350167,-5367.886884
+-4.961508,10033.058983,-55494.866523,148299.009937,-241628.020281,241936.619098,-99953.838107,-109503.905178,227759.623648,-153400.382066,-57871.955863,243812.285105,-284686.617848,196078.467857,-81029.159813,15782.954402
+-4.128535,4532.874809,-25745.113112,70876.346207,-120911.130404,130856.819632,-67543.630959,-44879.120077,126179.256077,-104009.637785,-14484.769866,141281.639761,-186818.019281,139669.139518,-61693.678613,12798.731167
@@ -0,0 +1,3 @@
+33.199367,138.158211,1760.713681,-9485.356597,20775.355572,-24394.374222,11022.993950,12043.617253,-24216.889279,12975.017872,10915.744792,-25108.453420,19912.090947,-6202.218691,-1558.928735,1372.621625
+-1.577474,11255.921476,-62528.511615,168920.215318,-280064.453770,288192.218751,-128206.255817,-122292.181709,274821.844107,-194699.491129,-62707.548151,297855.283959,-353627.428607,244271.805008,-100332.330615,19273.711817
+-5.232276,7150.642176,-39352.614462,103874.534052,-167384.012470,166428.279800,-68963.735603,-74209.056392,156402.730841,-107735.024304,-37109.268357,168521.885142,-200975.805149,140877.548496,-59181.720048,11767.244977
@@ -0,0 +1,3 @@
+34.614122,815.961926,-1154.434655,-4074.926602,16237.852320,-25421.655410,17512.513391,6849.609562,-26845.886359,21434.443242,5859.638022,-29910.962362,31662.834439,-16932.920460,3694.762609,214.658082
+-1.000321,6174.378368,-34366.376569,94953.230525,-163294.900698,177742.781111,-91230.156970,-63020.847394,172563.267754,-138133.950006,-25160.082116,192299.226737,-245013.260217,176986.057501,-75213.242200,14812.891679
+-5.868994,7923.296287,-43743.274861,115295.056066,-184359.193460,179939.705168,-69452.166823,-85858.131895,167973.226972,-108048.062601,-46935.644757,178340.547884,-203862.795949,138663.454881,-56837.053190,11072.305399
@@ -0,0 +1,3 @@
+26.274174,-35.445711,2200.712032,-9326.770362,18819.996632,-21546.216785,10512.810672,8819.557083,-20853.522932,14326.962931,5608.658825,-22395.307092,24418.707230,-14990.069709,5043.782779,-666.267695
+-3.887569,1356.038781,-6326.155274,15809.273891,-25140.634269,25249.443255,-10950.421146,-10801.263549,23996.177237,-17160.751928,-5224.492736,26183.279819,-31634.374975,22113.432181,-9085.547229,1654.322481
+-14.356673,-1060.265957,5394.145878,-13303.180729,20072.052768,-18434.734968,6154.889826,9634.740953,-17111.374829,10177.720206,5697.768311,-18213.579723,19545.275662,-12351.844267,4641.202624,-827.468340
@@ -0,0 +1,3 @@
+27.410096,-864.285016,6851.412485,-21350.203651,36748.146830,-36147.112391,11676.125630,20850.574429,-33741.113702,15615.747592,15988.201620,-33852.530525,28490.635303,-12774.633535,2180.330829,272.043093
+-2.746678,4712.801399,-25015.160393,65633.794977,-105879.897961,105340.913431,-42990.142655,-48381.577403,99488.890364,-66121.043877,-26557.658142,106612.071962,-122220.655684,82308.447444,-32988.048560,6118.951586
+-15.005835,-1012.800223,5256.805590,-13504.215719,21625.140329,-21602.912973,9071.909514,9734.213287,-20623.915646,13950.178465,5458.040914,-22239.632938,25334.921307,-16952.568064,6903.638306,-1380.370579
@@ -0,0 +1,3 @@
+29.005850,427.426465,186.089235,-4673.942632,11034.625029,-11612.898773,2299.065563,9293.363978,-10876.537185,413.581510,10162.276317,-9338.953291,-537.312907,7669.927208,-6552.450655,2061.728899
+-1.782697,5200.508968,-27990.038293,74567.066622,-122329.028981,124270.446343,-53428.076689,-54571.296413,118079.500037,-81413.303327,-28997.516048,127400.007067,-148760.795702,101333.834739,-40945.284151,7648.211115
+-16.396999,-464.621154,1997.704671,-4791.457255,8314.348255,-10349.969774,7481.169088,1261.140147,-10576.402396,11782.470755,-1580.977744,-12840.379938,20366.966244,-16939.807851,8298.183321,-1939.156604
@@ -0,0 +1,3 @@
+27.199352,-528.204958,3742.912985,-9913.718831,12926.475648,-5922.651791,-7504.426957,13319.469550,-3471.154708,-11894.353797,14344.176890,580.992388,-18064.283657,22061.615330,-13194.527033,3479.924962
+-4.196332,6414.445410,-34331.142537,89717.826767,-143543.239787,141199.844034,-56098.750517,-66003.233228,132695.304964,-87052.861005,-36067.095787,141765.953390,-162470.023307,109917.141395,-44440.873480,8378.601074
+-9.109570,526.734930,-2571.028565,6380.634656,-10789.260478,13213.847387,-10068.506209,-318.991193,13079.161800,-17059.936486,4785.488011,16921.105034,-30749.222302,26910.139672,-13070.615768,2854.649973
@@ -0,0 +1,3 @@
+30.646976,1629.862294,-7271.645262,17283.430815,-28379.407108,32772.027370,-21683.535397,-5316.738197,32329.791906,-35385.160596,5099.833300,38893.137320,-63616.893259,54407.003012,-27147.867828,6367.308854
+-2.509790,8236.757472,-44581.595066,117709.854480,-190329.008283,189517.832578,-77458.608951,-86867.278355,178884.002288,-119113.233369,-47447.488617,191706.992134,-220276.824798,148691.477782,-59754.583674,11170.294083
+-11.190316,367.201161,-1384.446493,1729.583280,547.172565,-4320.433214,5145.642118,-642.555399,-5209.186240,5981.348142,-444.066664,-6026.133303,8185.338547,-6011.059744,2779.439015,-671.650393
@@ -0,0 +1,3 @@
+32.809066,3240.065573,-14954.674327,34529.289194,-51059.348227,48938.507235,-21532.596158,-19026.382905,45878.447078,-37011.289024,-4910.666911,51266.024445,-70842.710220,56196.122555,-26908.129049,6178.439775
+-1.486918,6656.518783,-36426.201342,98405.872747,-164067.847629,170476.922192,-77804.446418,-70416.547315,163149.411429,-118021.767947,-35177.672485,177791.946851,-213213.900997,147826.185392,-60584.733237,11486.999247
+-13.133450,224.259873,-859.414770,446.876891,3604.085436,-10192.345703,12257.909428,-3545.498049,-11346.773013,18007.582976,-6739.049702,-15097.390065,29562.523585,-26767.242315,13720.587194,-3265.909871
@@ -0,0 +1,3 @@
+25.847112,1268.765892,-6585.935117,17988.370166,-32188.857279,37924.281861,-23399.158623,-9131.109727,37467.223653,-35759.631374,305.078839,43154.292098,-62872.062624,50350.761154,-24005.750364,5463.657034
+-5.248830,5789.509204,-30779.352192,79984.114434,-127495.795971,125365.305340,-50409.433698,-57521.790063,117652.347947,-79008.142823,-29896.843367,126262.546501,-148030.973316,102282.587307,-42241.197865,8131.152591
+-7.070734,3522.246467,-19306.602620,51369.522539,-84774.893353,88491.737638,-42638.620903,-33173.687605,84725.615938,-66720.264267,-12715.641616,94419.206750,-121121.946564,88254.275469,-37812.916887,7552.424638
@@ -0,0 +1,3 @@
+31.251162,3979.202387,-20299.845125,51522.329716,-82518.943564,84284.616798,-39541.936485,-32201.066944,80108.176049,-62913.347712,-11373.408630,88557.535207,-116232.473656,88032.389405,-40223.341699,8819.630022
+-2.677196,8361.488940,-45412.215200,120299.150008,-195366.326607,195881.193269,-81772.217381,-88029.340139,185284.123496,-125632.747569,-47077.464863,199273.882539,-231498.676929,157545.851862,-63768.807657,12003.674834
+-8.906908,1999.265732,-10742.703913,27882.758792,-44795.398472,45449.198232,-20854.533660,-17820.053812,43195.699032,-33357.300467,-6972.370084,48101.193085,-61375.873242,44549.435211,-18910.878149,3702.185243
@@ -0,0 +1,3 @@
+33.657829,3228.483519,-15022.389800,34816.013307,-51368.866400,48767.214351,-20820.338034,-19505.942978,45399.166679,-36006.086972,-5219.633139,50393.369850,-69623.211799,55602.900978,-26927.324454,6267.943668
+-1.725483,8835.043467,-48401.383614,129820.531178,-214234.982933,219672.356724,-97268.866171,-93472.035539,209412.561369,-148328.702066,-47872.758595,227284.469828,-269682.873639,185809.989314,-75857.318020,14384.049893
+-10.565018,1096.939398,-5179.080600,10791.209048,-11576.004549,3349.681312,8074.312560,-10839.177121,1029.709437,10830.230874,-10797.172349,-2128.682079,15149.217020,-16972.574434,9615.689302,-2430.161385
@@ -0,0 +1,3 @@
+34.736972,3190.507394,-14166.176198,30884.910182,-42194.566929,36052.387076,-11669.923752,-17502.218930,32472.945473,-22768.531678,-5928.191215,35199.999832,-47384.794872,38258.279146,-19111.730673,4636.122243
+-1.283728,6111.624715,-33582.409578,91860.820653,-156182.202705,167262.580808,-82540.357913,-62777.931804,161687.083101,-125013.762539,-27727.514343,178925.657581,-222813.541638,158175.337995,-66003.174280,12701.669168
+-11.962387,-1213.184184,6900.515987,-19760.880870,36052.018868,-42918.673237,26997.991268,9488.788715,-42227.590715,41639.813289,-1753.981409,-49236.227713,73503.482824,-59242.249431,27956.442140,-6199.345248
@@ -0,0 +1,3 @@
+29.192685,4132.924878,-21683.922637,56174.809607,-91014.014900,93151.547185,-42911.540392,-36870.273902,88496.681983,-67307.376497,-14866.803738,97032.815581,-123985.575539,92105.303145,-41416.530532,8965.927180
+-3.549593,8119.780731,-44017.302204,116195.801847,-187943.014291,187592.319287,-77622.084238,-84738.331969,177095.274712,-119770.022575,-45020.185247,190331.558121,-221576.612446,151367.147741,-61596.424017,11675.661644
+-6.952903,4943.960229,-27293.766654,72695.763028,-119165.867291,122160.520122,-55416.996110,-49733.755806,116302.209123,-86139.501717,-22604.284758,127647.492733,-157358.222673,111813.440234,-47033.786136,9267.605554
@@ -0,0 +1,3 @@
+33.127170,3169.216022,-15208.971727,36413.394628,-55433.036751,54238.962979,-24254.947815,-21019.363921,50839.645231,-40603.648828,-5839.290014,56371.664140,-77275.981053,61158.747813,-29341.409848,6770.047285
+-2.074269,9913.516687,-54456.881306,145785.047894,-239498.821224,243698.194214,-105552.064870,-106057.653131,231655.448115,-161074.162948,-55559.851382,250349.481150,-294152.543721,201506.315421,-81981.508038,15534.274009
+-8.382507,2565.395022,-13432.841653,33425.950184,-50146.597697,45026.891716,-13314.857811,-25286.710570,41193.107028,-22067.088783,-15609.334846,42771.024221,-43965.365321,27097.060931,-9825.696932,1606.803756
@@ -0,0 +1,3 @@
+34.377514,2604.716670,-11353.832025,24311.396536,-32753.847754,27914.601568,-9612.848219,-12524.998963,24993.101319,-19268.189302,-2546.549426,27544.563856,-40302.209943,34412.753870,-17941.006059,4490.725853
+-1.563054,8480.334139,-46729.542021,126437.630989,-211134.296882,220266.643974,-102038.453265,-89257.564595,211162.371544,-155141.817706,-43356.984393,230977.019677,-279685.992384,195285.988413,-80604.453797,15443.320886
+-9.624542,918.845731,-4363.978269,8975.208864,-8926.305293,562.632990,9926.218291,-10573.237174,-1677.595135,14233.520777,-11665.069052,-5658.065190,21435.858976,-22418.269583,12239.114971,-3000.426534
@@ -0,0 +1,3 @@
+18.644236,3664.504512,-18910.052428,47406.616989,-72593.958812,66763.877241,-20033.423313,-39186.042631,62879.857281,-30162.531932,-29180.255671,64565.368311,-56004.553026,26322.123288,-5677.399794,87.588777
+-2.528995,-8753.763501,49977.705155,-135403.600651,223932.134891,-230332.098007,104320.591397,94309.464496,-219226.453624,161175.904142,43986.049206,-240143.875640,293742.966807,-207302.854519,86747.190627,-17122.532582
+-5.040884,4198.062971,-22594.610444,58180.491072,-91437.599749,88711.911899,-35315.315132,-40289.857682,82743.051959,-56817.301938,-19341.604525,89256.093630,-107792.673876,76747.333267,-32858.481661,6681.145476
@@ -0,0 +1,3 @@
+31.015223,3191.701810,-15995.098483,39927.512319,-63031.923630,63597.356105,-29477.302102,-24266.761430,60077.060904,-47632.528848,-7678.339675,66386.687195,-89048.555505,69060.950608,-32472.203295,7353.602653
+-2.749667,10094.743211,-55142.234218,146327.105123,-237740.436599,238317.631355,-99347.807818,-107266.716910,225369.973139,-152550.506696,-57478.575458,242229.400637,-281172.210471,191355.958492,-77552.453741,14664.329118
+-6.632093,3748.718132,-20429.641854,53678.980244,-86758.297948,87547.573753,-38514.884985,-36526.478115,82855.945285,-60594.089407,-16504.099721,90691.187986,-111925.083344,79936.369223,-33848.234402,6711.623517
@@ -0,0 +1,3 @@
+33.767761,2168.945564,-9430.523610,20360.321407,-28155.801972,25452.673326,-10802.058834,-9341.211171,23192.237493,-20466.293068,43.465958,26262.829076,-41188.595841,36150.576264,-19056.760171,4781.246148
+-1.820943,10343.316962,-57178.300488,154089.207862,-255095.617455,262238.905477,-116540.356892,-111345.763462,250113.241130,-177218.564545,-57153.788391,271295.371892,-321825.603517,221834.240449,-90736.778907,17298.314630
+-7.745505,3063.351919,-16103.153297,40103.657176,-59852.391675,52763.059804,-13906.968400,-31507.901211,47878.371689,-23011.273164,-20555.184696,48745.120468,-46997.409063,27319.161432,-9319.222521,1417.719545
@@ -0,0 +1,3 @@
+34.322345,1974.509717,-7859.726512,14782.535894,-16520.209360,10352.811615,-654.394736,-6258.097283,7956.892883,-5657.228825,-63.883746,8592.085986,-16088.430887,16800.917252,-10348.701715,2948.328479
+-1.498902,7530.912981,-41512.827629,112782.973071,-189688.895781,200236.416550,-95753.195172,-78022.807746,192671.666628,-145588.948619,-35881.459729,212066.537227,-261216.385436,184531.683190,-76928.736265,14873.922504
+-8.749728,1948.586653,-10252.757606,25054.270588,-35546.027684,27536.306709,-1363.704606,-22761.999051,23777.220654,-2794.039174,-18289.439524,21487.443121,-9940.714843,-980.316955,3406.238588,-1261.576523
--- a/Show More
+++ b/Show More