added manual attitude control switch

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -173,6 +173,8 @@ FixedwingPositionINDIControl::parameters_update()
 
     _thrust = _param_thrust.get();
 
+    _switch_manual = _param_switch_manual.get();
+
 
 	// sanity check parameters
     // TODO: include sanity check
@@ -267,6 +269,14 @@ FixedwingPositionINDIControl::vehicle_status_poll()
     }
 }
 
+void
+FixedwingPositionINDIControl::manual_control_setpoint_poll()
+{
+    if(_switch_manual){
+        _manual_control_setpoint_sub.update(&_manual_control_setpoint);
+    }
+}
+
 void
 FixedwingPositionINDIControl::vehicle_attitude_poll()
 {
@@ -396,8 +406,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] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
+    //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] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
     //PX4_ERR(home_dir);
     strcat(home_dir,filename);
     FILE* fp = fopen(home_dir, "r");
@@ -564,7 +574,7 @@ FixedwingPositionINDIControl::Run()
         airspeed_poll();
         airflow_aoa_poll();
         airflow_slip_poll();
-
+        manual_control_setpoint_poll();
         vehicle_local_position_poll();
         vehicle_attitude_poll();
         vehicle_angular_velocity_poll();
@@ -1103,6 +1113,42 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve
         PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err*w_err), (double)q_err.angle(), (double)(q_err.axis()*q_err.axis()));
     }
 
+    // ====================================
+    // manual attitude setpoint feedthrough
+    // ====================================
+    if (_switch_manual){
+        // get an attitude setpoint from the current manual inputs
+        float roll_ref = 1.f * _manual_control_setpoint.y * M_PI_4_F;
+        float pitch_ref = -1.f* _manual_control_setpoint.x * 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));
+        Dcmf R_enu_frd_ref(_R_ned_to_enu*R_ned_frd_ref);
+        Quatf att_ref(R_enu_frd_ref);
+        R_ref = Dcmf(att_ref);
+
+        // get attitude error
+        R_ref_true = Dcmf(R_ref.transpose()*R_ib);
+        // get required rotation vector (in body frame)
+        q_err = AxisAnglef(R_ref_true);
+        // project rotation angle to [-pi,pi]
+        if (q_err.angle()*q_err.angle()<M_PI_F*M_PI_F){
+            w_err = -q_err.angle()*q_err.axis();
+        }
+        else{
+            if (q_err.angle()>0.f){
+                w_err = (2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis();
+            }
+            else{
+                w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis();
+            }
+        }
+
+        // compute rot acc command
+        rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-omega_filtered);
+        
+    }
+
     return rot_acc_command;
 }
 

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -121,6 +121,7 @@ private:
     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 _actuator_controls_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	
     // Publishers
 	uORB::Publication<actuator_controls_s>							_actuators_0_pub;
@@ -205,7 +206,9 @@ private:
 		// loiter params
 		(ParamInt<px4::params::DS_LOITER>) _param_loiter,
 		// thrust params
-		(ParamFloat<px4::params::DS_THRUST>) _param_thrust
+		(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
+		// RC feedthrough params
+		(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual
 
 	)
 
@@ -343,6 +346,8 @@ private:
 	int _loiter;
 	// thrust
 	float _thrust;
+	// controller mode
+	bool _switch_manual;
 
 	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.

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -542,7 +542,6 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8101000f);
  */
 PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f);
 
-
 // ======================================================
 // ==============  loiter circle number =================
 // ======================================================
@@ -572,4 +571,20 @@ PARAM_DEFINE_INT32(DS_LOITER, 0);
  * @increment 0.1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_THRUST, 0);
+PARAM_DEFINE_FLOAT(DS_THRUST, 0);
+
+// ======================================================
+// ================ Stick feedthorugh ===================
+// ======================================================
+
+/**
+ * integer in {0,1} defining if manual attitude setpoints are commanded by the pilot, 0=DS-controller, 1=manual feedthrough
+ * 
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);