dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FixedWingPositionControl: Use reference model for all auto modes except when in vtol transition

This commit is contained in:
Konrad 2023-10-13 13:58:37 +02:00
parent 8212cc49af
commit 1b83d720b2
4 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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

1
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -204,6 +204,7 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_ref_sp_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};

16
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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;

5
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -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<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_ref_sp_pub{ORB_ID(vehicle_attitude_reference_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _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