From 4525aa18d91b120e1bc059bbee7b0d0727530d9e Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Tue, 19 Apr 2022 17:50:33 +0200 Subject: [PATCH] added offboard mode to dyn_soar_controller --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- .../FixedwingPositionINDIControl.cpp | 56 ++++++++++++------- .../FixedwingPositionINDIControl.hpp | 6 +- 3 files changed, 42 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 655fa50cff..feeda50914 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,7 +13,7 @@ ekf2 start & # # Start attitude controller. # -#fw_att_control start +fw_att_control start fw_pos_control_l1 start fw_dyn_soar_control start airspeed_selector start diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 6337203937..8483c10ff0 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -207,6 +207,14 @@ FixedwingPositionINDIControl::airflow_slip_poll() _slip_valid = slip_valid; } +void +FixedwingPositionINDIControl::vehicle_status_poll() +{ + if(_vehicle_status_sub.update(&_vehicle_status)){ + print_message(_vehicle_status); + } +} + void FixedwingPositionINDIControl::vehicle_attitude_poll() { @@ -264,10 +272,10 @@ FixedwingPositionINDIControl::soaring_controller_status_poll() { if (_soaring_controller_status_sub.update(&_soaring_controller_status)){ if (!_soaring_controller_status.soaring_controller_running){ - PX4_INFO("Soaring controller turned off"); + //PX4_INFO("Soaring controller turned off"); } if (_soaring_controller_status.timeout_detected){ - PX4_INFO("Controller timeout detected"); + //PX4_INFO("Controller timeout detected"); } } } @@ -371,6 +379,7 @@ FixedwingPositionINDIControl::Run() // run polls _set_wind_estimate(Vector3f(0.f,0.f,0.f)); + vehicle_status_poll(); airspeed_poll(); airflow_aoa_poll(); airflow_slip_poll(); @@ -403,7 +412,7 @@ FixedwingPositionINDIControl::Run() Vector3f pos_ref = _pos + Vector3f{1.f,0.f,1.f}; // in inertial ENU Vector3f vel_ref = Vector3f{15.f,0.f,0.f}; // in inertial ENU Vector3f acc_ref = Vector3f{0.f,0.f,0.f}; // gravity-corrected acceleration (ENU) - Quatf q = _get_attitude_ref(0.f,10.f); + //Quatf q = _get_attitude_ref(0.f,10.f); Vector3f omega_ref = Vector3f{0.f,0.f,0.f}; // body angular velocity Vector3f alpha_ref = Vector3f{0.f,0.f,0.f}; // body angular acceleration @@ -414,6 +423,15 @@ FixedwingPositionINDIControl::Run() // ===================== Vector3f ctrl = _compute_NDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + // ================================= + // publish offboard control commands + // ================================= + offboard_control_mode_s ocm{}; + ocm.actuator = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + /* // ===================== // publish control input // ===================== @@ -449,6 +467,7 @@ FixedwingPositionINDIControl::Run() _angular_vel_sp.pitch = omega_ref(1); _angular_vel_sp.yaw = omega_ref(2); _angular_vel_sp_pub.publish(_angular_vel_sp); + */ // ============================ // compute actuator deflections @@ -459,15 +478,18 @@ FixedwingPositionINDIControl::Run() // ========================= // publish acutator controls // ========================= - _actuators = {}; - _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = hrt_absolute_time(); - _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); - _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); - _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - _actuators_0_pub.publish(_actuators); - //print_message(_actuators); + // Publish actuator controls only once in OFFBOARD + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + _actuators = {}; + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = hrt_absolute_time(); + _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); + _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); + _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; + _actuators_0_pub.publish(_actuators); + print_message(_actuators); + } // =========================== // publish rate control status @@ -479,13 +501,6 @@ FixedwingPositionINDIControl::Run() rate_ctrl_status.yawspeed_integ = 0.0f; _rate_ctrl_status_pub.publish(rate_ctrl_status); - /* - PX4_INFO("control setpoints:\t%.4f\t%.4f\t%.4f", - (double)vec(0), - (double)vec(1), - (double)vec(2)); - */ - // ============================== // publish soaring control status // ============================== @@ -493,7 +508,8 @@ FixedwingPositionINDIControl::Run() _soaring_controller_heartbeat.timestamp = hrt_absolute_time(); _soaring_controller_heartbeat.heartbeat = hrt_absolute_time(); _soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat); - //print_message(_soaring_controller_heartbeat); + + } perf_end(_loop_perf); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 879e91d489..4516757308 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -105,6 +106,7 @@ private: 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 @@ -120,7 +122,8 @@ private: uORB::Publication _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; + uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; // Message structs vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; @@ -134,6 +137,7 @@ private: vehicle_angular_acceleration_s _angular_accel {}; ///< vehicle angular acceleration home_position_s _home_pos {}; ///< home position 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