From 8714b8e92a8b442b022bfa5c06d6d97b4cbba427 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Apr 2015 12:10:33 +0200 Subject: improve mavlink set_attitude_target handling port #1920 to mavlink_receiver fixes #1921 --- src/modules/mavlink/mavlink_receiver.cpp | 48 +++++++++++++++++--------------- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3f9f7e139..e66cf4d0b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_alt0(0.0f), _hil_local_proj_ref{}, _offboard_control_mode{}, + _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0) @@ -783,16 +784,16 @@ 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 = (bool)(set_attitude_target.type_mask & 0x7); - bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate_msg && 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 && _offboard_control_mode.ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; } else { - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg; + _offboard_control_mode.ignore_attitude = ignore_attitude_msg; } _offboard_control_mode.ignore_position = true; @@ -821,22 +822,23 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(_offboard_control_mode.ignore_attitude)) { - static struct vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); - att_sp.R_valid = true; + _att_sp.timestamp = hrt_absolute_time(); + if (!ignore_attitude_msg) { // only copy att sp if message contained new data + mavlink_quaternion_to_euler(set_attitude_target.q, + &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body); + _att_sp.R_valid = true; + _att_sp.yaw_sp_move_rate = 0.0; + memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d)); + _att_sp.q_d_valid = true; + } if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - att_sp.thrust = set_attitude_target.thrust; + _att_sp.thrust = set_attitude_target.thrust; } - att_sp.yaw_sp_move_rate = 0.0; - memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); - att_sp.q_d_valid = true; if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } } @@ -844,9 +846,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) ///XXX add support for ignoring individual axes if (!(_offboard_control_mode.ignore_bodyrate)) { _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = set_attitude_target.body_roll_rate; - _rates_sp.pitch = set_attitude_target.body_pitch_rate; - _rates_sp.yaw = set_attitude_target.body_yaw_rate; + if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data + _rates_sp.roll = set_attitude_target.body_roll_rate; + _rates_sp.pitch = set_attitude_target.body_pitch_rate; + _rates_sp.yaw = set_attitude_target.body_yaw_rate; + } if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid _rates_sp.thrust = set_attitude_target.thrust; } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ffacb59a6..2d619e822 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -184,6 +184,7 @@ private: float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; struct offboard_control_mode_s _offboard_control_mode; + struct vehicle_attitude_setpoint_s _att_sp; struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; -- cgit v1.2.3