From dcb680f9d6f16edee276df81b71a226164976a21 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 25 Jun 2015 22:04:23 +0200 Subject: [PATCH] VTOL: only publish attitude setpoint if in correct mode --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) 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 95c8545e73..b5436602ec 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 @@ -1762,11 +1762,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub > 0) { + if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else { + } else if (_attitude_sp_pub <= 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } 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 6eaca26b17..ccae79c11d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -119,6 +119,7 @@ private: int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ @@ -134,6 +135,7 @@ 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 */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ @@ -316,6 +318,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_alt_sp(true), _mode_auto(false) { + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); @@ -471,6 +474,12 @@ MulticopterPositionControl::poll_subscriptions() { bool updated; + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + orb_check(_att_sub, &updated); if (updated) { @@ -900,6 +909,7 @@ MulticopterPositionControl::task_main() /* * do subscriptions */ + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1432,10 +1442,9 @@ MulticopterPositionControl::task_main() if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub > 0) { + if (_att_sp_pub > 0 && _vehicle_status.is_rotary_wing) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { + } else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } }