From adbe15ee9e09b423da8f8d50c1abf9eba16123d4 Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 13 Oct 2023 13:58:37 +0200 Subject: [PATCH] FixedWingPositionControl: Use reference model for all auto modes except when in vtol transition --- .../fw_pos_control/FixedwingPositionControl.cpp | 11 ++++++++++- .../fw_pos_control/FixedwingPositionControl.hpp | 1 + .../vtol_att_control/vtol_att_control_main.cpp | 16 ++++++++++++++++ .../vtol_att_control/vtol_att_control_main.h | 5 +++++ 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a1286db402..5213a0edfc 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -52,6 +52,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _attitude_ref_sp_pub(vtol ? ORB_ID(fw_virtual_vehicle_attitude_reference_setpoint) : ORB_ID( + vehicle_attitude_reference_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _launchDetector(this), _runway_takeoff(this) @@ -2574,7 +2576,14 @@ FixedwingPositionControl::Run() q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); - _attitude_sp_pub.publish(_att_sp); + + // Enable reference model use for auto modes except when transitioning. + if (_control_mode_current == FW_POSCTRL_MODE_AUTO && !_vehicle_status.in_transition_mode) { + _attitude_ref_sp_pub.publish(_att_sp); + + } else { + _attitude_sp_pub.publish(_att_sp); + } // only publish status in full FW mode if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index eb42160483..35b934f2ac 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -216,6 +216,7 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _attitude_sp_pub; + uORB::Publication _attitude_ref_sp_pub; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Publication _npfg_status_pub{ORB_ID(npfg_status)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 199dcac84c..138c4b6a8c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -369,6 +369,18 @@ VtolAttitudeControl::Run() // check if mc and fw sp were updated const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + const bool fw_att_ref_sp_updated = _fw_virtual_att_ref_sp_sub.update(&_fw_virtual_att_ref_sp); + + if (fw_att_sp_updated) { + _is_in_att_ref_mode = false; + + } else if (fw_att_ref_sp_updated) { + _is_in_att_ref_mode = true; + } + + if (_is_in_att_ref_mode) { + _fw_virtual_att_sp = _fw_virtual_att_ref_sp; + } // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); @@ -415,6 +427,10 @@ VtolAttitudeControl::Run() if (fw_att_sp_updated) { _vtol_type->update_fw_state(); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); + + } else if (fw_att_ref_sp_updated) { + _vtol_type->update_fw_state(); + _vehicle_attitude_ref_sp_pub.publish(_vehicle_attitude_sp); } break; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 6ef046f05f..6cc4d4ea03 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -157,6 +157,7 @@ private: uORB::Subscription _action_request_sub{ORB_ID(action_request)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; + uORB::Subscription _fw_virtual_att_ref_sp_sub{ORB_ID(fw_virtual_vehicle_attitude_reference_setpoint)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; @@ -173,6 +174,7 @@ private: uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; uORB::Publication _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vehicle_attitude_ref_sp_pub{ORB_ID(vehicle_attitude_reference_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::PublicationMulti _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -183,6 +185,7 @@ private: vehicle_attitude_setpoint_s _vehicle_attitude_sp{}; // vehicle attitude setpoint vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint + vehicle_attitude_setpoint_s _fw_virtual_att_ref_sp{}; // virtual fw attitude setpoint vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint vehicle_torque_setpoint_s _vehicle_torque_setpoint_virtual_mc{}; @@ -209,6 +212,8 @@ private: float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3] + bool _is_in_att_ref_mode{false}; + #if !defined(ENABLE_LOCKSTEP_SCHEDULER) hrt_abstime _last_run_timestamp {0}; #endif // !ENABLE_LOCKSTEP_SCHEDULER