From 1b83d720b2c88f8c86bec344d2abe76ad6516c67 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 46a0c5e27a..f494344976 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) @@ -2435,7 +2437,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 dc252fea30..337c54f3e0 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -204,6 +204,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 eccdf78305..6e0453e8c2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -378,6 +378,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(); @@ -424,6 +436,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 132448177f..1179699247 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -159,6 +159,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)}; @@ -175,6 +176,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)}; @@ -185,6 +187,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{}; @@ -211,6 +214,8 @@ private: float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3] + bool _is_in_att_ref_mode{false}; + hrt_abstime _last_run_timestamp{0}; /* For multicopters it is usual to have a non-zero idle speed of the engines