Rebase fixes

Initialize local origin

F

F

Disable manual switch

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -13,7 +13,7 @@ control_allocator start
 #
 # Start attitude controller.
 #
-fw_indi_pos_control start
+fw_dyn_soar_control start
 fw_rate_control start
 fw_att_control start
 fw_mode_manager start

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -53,7 +53,6 @@ using matrix::wrap_pi;
 FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-    _actuators_0_pub(ORB_ID(actuator_controls_0)),
     _attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
@@ -172,10 +171,11 @@ FixedwingPositionINDIControl::parameters_update()
     // check if local NED reference frame origin has changed:
     // || (_local_pos.vxy_reset_counter != _pos_reset_counter
     // initialize projection
-    map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+    _global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
-                    _local_pos.ref_timestamp);
+        _local_pos.ref_timestamp);
-    // project the origin of the soaring ENU frame to the current NED frame
+    matrix::Vector2f origin_NE = _global_local_proj_ref.project(_origin_lat, _origin_lon);
-    map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E);
+    _origin_N = origin_NE(0);
+    _origin_E = origin_NE(1);
     _origin_D =  _local_pos.ref_alt - _origin_alt;
     PX4_INFO("local reference frame updated");
 
@@ -230,58 +230,6 @@ FixedwingPositionINDIControl::airspeed_poll()
     _airspeed_valid = airspeed_valid;
 }
 
-void
-FixedwingPositionINDIControl::airflow_aoa_poll()
-{
-	bool aoa_valid = _aoa_valid;
-	airflow_aoa_s aoa_validated;
-
-	if (_airflow_aoa_sub.update(&aoa_validated)) {
-
-		if (PX4_ISFINITE(aoa_validated.aoa_rad)
-		    && (aoa_validated.valid)) {
-
-			aoa_valid = true;
-
-			_aoa_last_valid = aoa_validated.timestamp;
-			_aoa = aoa_validated.aoa_rad;
-		}
-
-	} else {
-		// no aoa updates for one second
-		if (aoa_valid && (hrt_elapsed_time(&_aoa_last_valid) > 1_s)) {
-			aoa_valid = false;
-		}
-	}
-    _aoa_valid = aoa_valid;
-}
-
-void
-FixedwingPositionINDIControl::airflow_slip_poll()
-{
-	bool slip_valid = _slip_valid;
-	airflow_slip_s slip_validated;
-
-	if (_airflow_slip_sub.update(&slip_validated)) {
-
-		if (PX4_ISFINITE(slip_validated.slip_rad)
-		    && (slip_validated.valid)) {
-
-			slip_valid = true;
-
-			_slip_last_valid = slip_validated.timestamp;
-			_slip = slip_validated.slip_rad;
-		}
-
-	} else {
-		// no aoa updates for one second
-		if (slip_valid && (hrt_elapsed_time(&_slip_last_valid) > 1_s)) {
-			slip_valid = false;
-		}
-	}
-    _slip_valid = slip_valid;
-}
-
 void
 FixedwingPositionINDIControl::vehicle_status_poll()
 {
@@ -295,7 +243,7 @@ FixedwingPositionINDIControl::manual_control_setpoint_poll()
 {
     if(_switch_manual){
         _manual_control_setpoint_sub.update(&_manual_control_setpoint);
-        _thrust = _manual_control_setpoint.z;
+        _thrust = _manual_control_setpoint.throttle;
     }
     else {
         _thrust = _thrust_pos;
@@ -339,22 +287,12 @@ FixedwingPositionINDIControl::vehicle_angular_velocity_poll()
     // no need to check if it was updated as the main loop is fired based on an update...
     //
     _omega = Vector3f(_angular_vel.xyz);
+    _alpha = Vector3f(_angular_vel.xyz_derivative);
     if(hrt_absolute_time()-_angular_vel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){
         PX4_ERR("angular velocity sample is too old");
     }
 }
 
-void
-FixedwingPositionINDIControl::vehicle_angular_acceleration_poll()
-{
-	if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) {
-		_alpha = Vector3f(_angular_accel.xyz);
-    }
-    if(hrt_absolute_time()-_angular_accel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){
-        PX4_ERR("angular acceleration sample is too old");
-    }
-}
-
 void
 FixedwingPositionINDIControl::vehicle_local_position_poll()
 {
@@ -654,8 +592,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
     // =======================================================================
     bool error = false;
 
-    //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
+    char home_dir[200] = "/home/jaeyoung/src/PX4-Autopilot/src/modules/fw_dyn_soar_control/trajectories/";
-    char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
+    // char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
     //PX4_ERR(home_dir);
     strcat(home_dir,filename);
     FILE* fp = fopen(home_dir, "r");
@@ -806,15 +744,27 @@ FixedwingPositionINDIControl::Run()
 
         // check if local NED reference frame origin has changed:
         // || (_local_pos.vxy_reset_counter != _pos_reset_counter
-        if (!map_projection_initialized(&_global_local_proj_ref)
+        if (!_global_local_proj_ref.isInitialized()
-            || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)
+            || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
 		    || (_local_pos.xy_reset_counter != _pos_reset_counter)
             || (_local_pos.z_reset_counter != _alt_reset_counter)) {
+
+            double reference_latitude = 0.;
+            double reference_longitude = 0.;
+
+            if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
+                reference_latitude = _local_pos.ref_lat;
+                reference_longitude = _local_pos.ref_lon;
+            }
+
             // initialize projection
-            map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+			_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
-                            _local_pos.ref_timestamp);
+                _local_pos.ref_timestamp);
-            // project the origin of the soaring ENU frame to the current NED frame
+
-            map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E);
+            matrix::Vector2f origin_NE = _global_local_proj_ref.project(_origin_lat, _origin_lon);
+            _origin_N = origin_NE(0);
+            _origin_E = origin_NE(1);
+
             _origin_D =  _local_pos.ref_alt - _origin_alt;
             PX4_INFO("local reference frame updated");
         }
@@ -825,15 +775,12 @@ FixedwingPositionINDIControl::Run()
         // run polls
         vehicle_status_poll();
         airspeed_poll();
-        airflow_aoa_poll();
-        airflow_slip_poll();
         rc_channels_poll();
         manual_control_setpoint_poll();
         vehicle_local_position_poll();
         vehicle_attitude_poll();
         vehicle_acceleration_poll();
         vehicle_angular_velocity_poll();
-        vehicle_angular_acceleration_poll();
         soaring_controller_status_poll();
 
         // update the shear estimate, only target airspeed is updated in soaring mode
@@ -898,7 +845,7 @@ FixedwingPositionINDIControl::Run()
         // publish offboard control commands
         // =================================
         offboard_control_mode_s ocm{};
-        ocm.actuator = true;
+        ocm.thrust_and_torque = true;
         ocm.timestamp = hrt_absolute_time();
         _offboard_control_mode_pub.publish(ocm);
 
@@ -966,14 +913,13 @@ FixedwingPositionINDIControl::Run()
             // =========================
             // publish acutator controls
             // =========================
-            //_actuators = {};
+            _actuators = {};
             _actuators.timestamp = hrt_absolute_time();
             _actuators.timestamp_sample = hrt_absolute_time();
-            _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0);
+            ctrl2.copyTo(_actuators.xyz);
-            _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1);
+            _torque_sp_pub.publish(_actuators);
-            _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2);
+            _thrust_sp.xyz[0] = _thrust;
-            _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _thrust;
+            _thrust_sp_pub.publish(_thrust_sp);
-            _actuators_0_pub.publish(_actuators);
             //print_message(_actuators);
 
             // =====================
@@ -1447,8 +1393,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
     // ====================================
     if (_switch_manual){
         // get an attitude setpoint from the current manual inputs
-        float roll_ref = 1.f * _manual_control_setpoint.y * 1.0f;
+        float roll_ref = 1.f * _manual_control_setpoint.roll * 1.0f;
-        float pitch_ref = -1.f* _manual_control_setpoint.x * M_PI_4_F;
+        float pitch_ref = -1.f* _manual_control_setpoint.pitch * M_PI_4_F;
         Eulerf E_current(Quatf(_attitude.q));
         float yaw_ref = E_current.psi();
         Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref));
@@ -1521,8 +1467,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl)
 
     // compute moments
     Vector3f moment;
-    moment(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0);
+    moment(0) = _k_ail*q*_actuators.xyz[0] - _k_d_roll*q*_omega(0);
-    moment(1) = _k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - _k_d_pitch*q*_omega(1);
+    moment(1) = _k_ele*q*_actuators.xyz[1] - _k_d_pitch*q*_omega(1);
     moment(2) = 0.f;
 
     // introduce artificial time delay that is also present in acceleration

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -18,13 +18,13 @@
 #include <string.h>
 
 #include <drivers/drv_hrt.h>
-#include "fw_att_control/ecl_pitch_controller.h"
+#include "fw_att_control/fw_pitch_controller.h"
-#include "fw_att_control/ecl_roll_controller.h"
+#include "fw_att_control/fw_roll_controller.h"
-#include "fw_att_control/ecl_wheel_controller.h"
+#include "fw_att_control/fw_wheel_controller.h"
-#include "fw_att_control/ecl_yaw_controller.h"
+#include "fw_att_control/fw_yaw_controller.h"
-#include <lib/ecl/geo/geo.h>
+#include <lib/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
-#include <lib/landing_slope/Landingslope.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>
@@ -41,8 +41,8 @@
 #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_aoa.h>
-#include <uORB/topics/airflow_slip.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>
@@ -51,7 +51,6 @@
 #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>
@@ -62,8 +61,8 @@
 #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/vehicle_thrust_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/soaring_controller_heartbeat.h>
 #include <uORB/topics/soaring_controller_position_setpoint.h>
 #include <uORB/topics/soaring_controller_position.h>
@@ -118,21 +117,22 @@ private:
     // 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_aoa_sub{ORB_ID(airflow_aoa)};                           // angle of attack
-    uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)};                         // angle of sideslip
+//     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 _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 _actuator_controls_sub{ORB_ID(vehicle_torque_setpoint)};
 	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_torque_setpoint_s>				_torque_sp_pub{ORB_ID(vehicle_torque_setpoint)};
+	uORB::Publication<vehicle_thrust_setpoint_s>				_thrust_sp_pub{ORB_ID(vehicle_thrust_setpoint)};
 	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)};
@@ -146,7 +146,8 @@ private:
 
     // Message structs
 	vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
-	actuator_controls_s			_actuators {};			// actuator commands
+	vehicle_torque_setpoint_s			_actuators {};			// actuator commands
+	vehicle_thrust_setpoint_s			_thrust_sp {};
 	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
@@ -155,9 +156,9 @@ private:
 	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
+	matrix::Vector3f	_angular_accel {};	///< vehicle angular acceleration
 	home_position_s				_home_pos {};			///< home position
-	map_projection_reference_s 	_global_local_proj_ref{};
+	MapProjection 	_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
@@ -247,14 +248,13 @@ private:
 	// Update subscriptions
 	void        wind_poll();
 	void		airspeed_poll();
-	void		airflow_aoa_poll();
+	// void		airflow_aoa_poll();
-	void 		airflow_slip_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();
@@ -330,21 +330,21 @@ private:
 	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<float> _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<float> _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
+	math::LowPassFilter2p<float> _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<float> _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
+	math::LowPassFilter2p<float> _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<float> _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
+	math::LowPassFilter2p<float> _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<float> _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
+	math::LowPassFilter2p<float> _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};
 

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -562,7 +562,7 @@ PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
  * @increment 0.01
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_THRUST, 0);
+PARAM_DEFINE_FLOAT(DS_THRUST, 0.7);
 
 // ======================================================
 // ============= controller force saturation ============
@@ -624,7 +624,7 @@ PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1);
  * @increment 1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1);
+PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
 
 // =====================================================
 // ============= open loop / closed loop DS ============