From ac002db25c1a7eb3652ac39fd3b54f8acb67792d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 24 May 2019 08:01:56 +0100 Subject: [PATCH] MAVLink/Commander: @Pedro-Roque's offboard yawrate handling --- msg/offboard_control_mode.msg | 3 ++ msg/vehicle_control_mode.msg | 1 + src/modules/commander/Commander.cpp | 11 ++++- src/modules/mavlink/mavlink_receiver.cpp | 47 +++++++++++++++---- .../mc_att_control/mc_att_control_main.cpp | 8 ++++ 5 files changed, 59 insertions(+), 11 deletions(-) diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg index ab55684ec6..34c1f99580 100644 --- a/msg/offboard_control_mode.msg +++ b/msg/offboard_control_mode.msg @@ -5,6 +5,9 @@ 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 bool ignore_position bool ignore_velocity bool ignore_acceleration_force diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index cda7a4705a..e241b85695 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -8,6 +8,7 @@ bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_yawrate_override_enabled # true if the yaw rate override is enabled bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled bool flag_control_force_enabled # true if force control is mixed in bool flag_control_acceleration_enabled # true if acceleration is controlled diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 15a7c199d6..f575b9be9f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3322,7 +3322,10 @@ set_control_mode() * The control flags depend on what is ignored according to the offboard control mode topic * 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 || + 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 || @@ -3333,6 +3336,12 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; + 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; + control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f421b92b7f..b403c88537 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -826,8 +826,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* 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 = (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); bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000); bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); @@ -938,7 +940,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { + if (!(offboard_control_mode.ignore_bodyrate_x || + offboard_control_mode.ignore_bodyrate_y || + offboard_control_mode.ignore_bodyrate_z)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -994,7 +998,9 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m /* ignore all since we are setting raw actuators here */ offboard_control_mode.ignore_thrust = true; offboard_control_mode.ignore_attitude = true; - offboard_control_mode.ignore_bodyrate = true; + offboard_control_mode.ignore_bodyrate_x = true; + offboard_control_mode.ignore_bodyrate_y = true; + offboard_control_mode.ignore_bodyrate_z = true; offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; @@ -1289,16 +1295,28 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and * body rates to keep the controllers running */ - bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & 0x1); + bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & 0x2); + bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & 0x4); bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) { + + if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y || + ignore_bodyrate_msg_z) && + ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_bodyrate_x = + ignore_bodyrate_msg_x && _offboard_control_mode.ignore_bodyrate_x; + _offboard_control_mode.ignore_bodyrate_y = + ignore_bodyrate_msg_y && _offboard_control_mode.ignore_bodyrate_y; + _offboard_control_mode.ignore_bodyrate_z = + ignore_bodyrate_msg_z && _offboard_control_mode.ignore_bodyrate_z; _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; } else { - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg; + _offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; + _offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; + _offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; _offboard_control_mode.ignore_attitude = ignore_attitude_msg; } @@ -1357,13 +1375,22 @@ 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)) { + if (!_offboard_control_mode.ignore_bodyrate_x || + !_offboard_control_mode.ignore_bodyrate_y || + !_offboard_control_mode.ignore_bodyrate_z) { vehicle_rates_setpoint_s rates_sp = {}; rates_sp.timestamp = hrt_absolute_time(); - if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data + // only copy att rates sp if message contained new data + if (!ignore_bodyrate_msg_x) { rates_sp.roll = set_attitude_target.body_roll_rate; + } + + if (!ignore_bodyrate_msg_y) { rates_sp.pitch = set_attitude_target.body_pitch_rate; + } + + if (!ignore_bodyrate_msg_z) { rates_sp.yaw = set_attitude_target.body_yaw_rate; } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 2f708416b3..e455b4776a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -837,6 +837,14 @@ MulticopterAttitudeControl::run() } control_attitude(); + if (_v_control_mode.flag_control_yawrate_override_enabled) + { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + vehicle_rates_setpoint_poll(); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; + } + publish_rates_setpoint(); }