diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index f69dab4e0e..8840aef7d2 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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()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; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index a8ab3056bb..56ab469645 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/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 _actuators_0_pub; @@ -205,7 +206,9 @@ private: // loiter params (ParamInt) _param_loiter, // thrust params - (ParamFloat) _param_thrust + (ParamFloat) _param_thrust, + // RC feedthrough params + (ParamInt) _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. diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index af63df0f9b..067bee9f7b 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/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); \ No newline at end of file +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); \ No newline at end of file