diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8772ceb835..1328bca48d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ @@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rates_sp_id(0), _actuators_id(0), + _attitude_setpoint_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll() if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 24cf1f084f..8b4e4224eb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -166,6 +167,8 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_id_t _attitude_setpoint_id; + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), +/* publication ID */ + _attitude_setpoint_id(0), + /* states */ _ctrl_state(), _att_sp(), @@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } } @@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + } else if (_attitude_setpoint_id) { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /* XXX check if radius makes sense here */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 53eff5977f..a631336273 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,8 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + orb_id_t _attitude_setpoint_id; + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + _attitude_setpoint_id(0), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } orb_check(_ctrl_state_sub, &updated); @@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } else { @@ -1615,13 +1626,13 @@ MulticopterPositionControl::task_main() * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { - } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } }