From a707403eaf0a189edac485287a3c3a3ec54d7030 Mon Sep 17 00:00:00 2001 From: pedro-roque Date: Thu, 23 May 2019 15:24:12 +0200 Subject: [PATCH] mc_att_ctrl: added yawrate control from offboard. This commit adds Roll Pitch Yawrate Thrust (RPYrT) setpoint control to the PX4 stack, enabling the UAV to follow specific yawrates sent from offboard. It also introduces individual body_rate flags, along the lines of mavros. Tested on a MoCap enabled flight arena in KTH Royal Institute of Technology, Stockholm. The MAV receives RPYrT setpoints from an external PID controller to stabilize around position setpoints. The UAV is also externally disturbed to assess the stability to external unmodeled events. Fixed Kabir comments. Removed deprecated ignore_bodyrate. Fixed integration test. --- .../python_src/px4_it/util/manual_input.py | 4 ++- msg/offboard_control_mode.msg | 1 - src/modules/commander/Commander.cpp | 27 ++++++++++--------- src/modules/mavlink/mavlink_receiver.cpp | 3 --- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/integrationtests/python_src/px4_it/util/manual_input.py b/integrationtests/python_src/px4_it/util/manual_input.py index b7adaa31c4..d1381d99d6 100755 --- a/integrationtests/python_src/px4_it/util/manual_input.py +++ b/integrationtests/python_src/px4_it/util/manual_input.py @@ -129,7 +129,9 @@ class ManualInput(object): mode = offboard_control_mode() mode.ignore_thrust = ignore_thrust mode.ignore_attitude = ignore_attitude - mode.ignore_bodyrate = ignore_bodyrate + mode.ignore_bodyrate_x = ignore_bodyrate + mode.ignore_bodyrate_y = ignore_bodyrate + mode.ignore_bodyrate_z = ignore_bodyrate mode.ignore_position = ignore_position mode.ignore_velocity = ignore_velocity mode.ignore_acceleration_force = ignore_acceleration_force diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg index 34c1f99580..931c2fcf6d 100644 --- a/msg/offboard_control_mode.msg +++ b/msg/offboard_control_mode.msg @@ -4,7 +4,6 @@ uint64 timestamp # time since system start (microseconds) bool ignore_thrust bool ignore_attitude -bool ignore_bodyrate bool ignore_bodyrate_x bool ignore_bodyrate_y bool ignore_bodyrate_z diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f575b9be9f..834c758013 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1516,7 +1516,9 @@ Commander::run() if (old.ignore_thrust != offboard_control_mode.ignore_thrust || old.ignore_attitude != offboard_control_mode.ignore_attitude || - old.ignore_bodyrate != offboard_control_mode.ignore_bodyrate || + old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x || + old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y || + old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z || old.ignore_position != offboard_control_mode.ignore_position || old.ignore_velocity != offboard_control_mode.ignore_velocity || old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force || @@ -3323,24 +3325,25 @@ set_control_mode() * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) */ control_mode.flag_control_rates_enabled = - !offboard_control_mode.ignore_bodyrate_x || - !offboard_control_mode.ignore_bodyrate_y || - !offboard_control_mode.ignore_bodyrate_z || - !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + !offboard_control_mode.ignore_bodyrate_x || + !offboard_control_mode.ignore_bodyrate_y || + !offboard_control_mode.ignore_bodyrate_z || + !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || !offboard_control_mode.ignore_position || !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; + // TO-DO: Add support for other modes than yawrate control control_mode.flag_control_yawrate_override_enabled = - offboard_control_mode.ignore_bodyrate_x && - offboard_control_mode.ignore_bodyrate_y && - !offboard_control_mode.ignore_bodyrate_z && - !offboard_control_mode.ignore_attitude; + offboard_control_mode.ignore_bodyrate_x && + offboard_control_mode.ignore_bodyrate_y && + !offboard_control_mode.ignore_bodyrate_z && + !offboard_control_mode.ignore_attitude; control_mode.flag_control_rattitude_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b403c88537..36e37780cd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -825,8 +825,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); /* yaw ignore flag mapps to ignore_attitude */ offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); - /* yawrate ignore flag mapps to ignore_bodyrate */ - // offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800); @@ -1374,7 +1372,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ - ///XXX add support for ignoring individual axes if (!_offboard_control_mode.ignore_bodyrate_x || !_offboard_control_mode.ignore_bodyrate_y || !_offboard_control_mode.ignore_bodyrate_z) {