From b1ee89b5cad9ddccfa684cb31c01f5f82b26d6b4 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 8 Nov 2015 17:54:29 +0100 Subject: [PATCH 1/4] add nan protection for position target local --- src/modules/mavlink/mavlink_receiver.cpp | 33 +++++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4c849c14da63..291ab39a1414 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -642,11 +642,23 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t struct offboard_control_mode_s offboard_control_mode; memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) && + PX4_ISFINITE(set_position_target_local_ned.y) && + PX4_ISFINITE(set_position_target_local_ned.z) && + PX4_ISFINITE(set_position_target_local_ned.vx) && + PX4_ISFINITE(set_position_target_local_ned.vy) && + PX4_ISFINITE(set_position_target_local_ned.vz) && + PX4_ISFINITE(set_position_target_local_ned.afx) && + PX4_ISFINITE(set_position_target_local_ned.afy) && + PX4_ISFINITE(set_position_target_local_ned.afz) && + PX4_ISFINITE(set_position_target_local_ned.yaw); + /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || set_position_target_local_ned.target_system == 0) && (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0)) { + set_position_target_local_ned.target_component == 0) && + values_finite) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -733,7 +745,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude && PX4_ISFINITE(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_attitude) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; @@ -742,7 +754,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate && PX4_ISFINITE(set_position_target_local_ned.yaw)) { + if (!offboard_control_mode.ignore_bodyrate) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -751,13 +763,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } //XXX handle global pos setpoints (different MAV frames) - - if (_pos_sp_triplet_pub == nullptr) { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - } else { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + if (valid) { + if (_pos_sp_triplet_pub == nullptr) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } } } From 46ad873d0065712042c8fa44f4a2c335a42c5185 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 8 Nov 2015 18:26:10 +0100 Subject: [PATCH 2/4] add nan protection to attitude and actuator targets --- src/modules/mavlink/mavlink_receiver.cpp | 61 ++++++++++++++++-------- 1 file changed, 41 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 291ab39a1414..31c1ef58d4fb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -642,16 +642,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t struct offboard_control_mode_s offboard_control_mode; memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints - bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) && - PX4_ISFINITE(set_position_target_local_ned.y) && - PX4_ISFINITE(set_position_target_local_ned.z) && - PX4_ISFINITE(set_position_target_local_ned.vx) && - PX4_ISFINITE(set_position_target_local_ned.vy) && - PX4_ISFINITE(set_position_target_local_ned.vz) && - PX4_ISFINITE(set_position_target_local_ned.afx) && - PX4_ISFINITE(set_position_target_local_ned.afy) && - PX4_ISFINITE(set_position_target_local_ned.afz) && - PX4_ISFINITE(set_position_target_local_ned.yaw); + bool values_finite = + PX4_ISFINITE(set_position_target_local_ned.x) && + PX4_ISFINITE(set_position_target_local_ned.y) && + PX4_ISFINITE(set_position_target_local_ned.z) && + PX4_ISFINITE(set_position_target_local_ned.vx) && + PX4_ISFINITE(set_position_target_local_ned.vy) && + PX4_ISFINITE(set_position_target_local_ned.vz) && + PX4_ISFINITE(set_position_target_local_ned.afx) && + PX4_ISFINITE(set_position_target_local_ned.afy) && + PX4_ISFINITE(set_position_target_local_ned.afz) && + PX4_ISFINITE(set_position_target_local_ned.yaw); /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || @@ -763,14 +764,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } //XXX handle global pos setpoints (different MAV frames) - if (valid) { - if (_pos_sp_triplet_pub == nullptr) { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - } else { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); - } + if (_pos_sp_triplet_pub == nullptr) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); } } @@ -793,10 +792,21 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints + bool values_finite = + PX4_ISFINITE(set_actuator_control_target.controls[0]) && + PX4_ISFINITE(set_actuator_control_target.controls[1]) && + PX4_ISFINITE(set_actuator_control_target.controls[2]) && + PX4_ISFINITE(set_actuator_control_target.controls[3]) && + PX4_ISFINITE(set_actuator_control_target.controls[4]) && + PX4_ISFINITE(set_actuator_control_target.controls[5]) && + PX4_ISFINITE(set_actuator_control_target.controls[6]) && + PX4_ISFINITE(set_actuator_control_target.controls[7]); + if ((mavlink_system.sysid == set_actuator_control_target.target_system || set_actuator_control_target.target_system == 0) && (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0)) { + set_actuator_control_target.target_component == 0) && + values_finite) { /* ignore all since we are setting raw actuators here */ offboard_control_mode.ignore_thrust = true; @@ -886,11 +896,22 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); + bool values_finite = + PX4_ISFINITE(set_attitude_target.q[0]) && + PX4_ISFINITE(set_attitude_target.q[1]) && + PX4_ISFINITE(set_attitude_target.q[2]) && + PX4_ISFINITE(set_attitude_target.q[3]) && + PX4_ISFINITE(set_attitude_target.thrust) && + PX4_ISFINITE(set_attitude_target.body_roll_rate) && + PX4_ISFINITE(set_attitude_target.body_pitch_rate) && + PX4_ISFINITE(set_attitude_target.body_yaw_rate); + /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || set_attitude_target.target_system == 0) && (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0)) { + set_attitude_target.target_component == 0) && + values_finite) { /* set correct ignore flags for thrust field: copy from mavlink message */ _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); From 664b0ee0a9b705320a807e621094b5719f9f4fae Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 15 Nov 2015 13:54:41 +0100 Subject: [PATCH 3/4] read setpoint type from type mask in offboard --- src/modules/mavlink/mavlink_receiver.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 31c1ef58d4fb..26bc3c079629 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -671,6 +671,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* yawrate ignore flag mapps to ignore_bodyrate */ offboard_control_mode.ignore_bodyrate = (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); + bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); + offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub == nullptr) { @@ -709,7 +714,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others + + if (is_takeoff_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + } else if(is_land_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + } else if(is_loiter_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } else { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } /* set the local pos values */ if (!offboard_control_mode.ignore_position) { From f3a1abf6590ed0ef4e1a263350f699bc6dfb26c4 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 26 Nov 2015 14:14:33 +0100 Subject: [PATCH 4/4] added support for SP type idle --- src/modules/mavlink/mavlink_receiver.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 26bc3c079629..a5ad8fab6604 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -642,7 +642,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t struct offboard_control_mode_s offboard_control_mode; memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints - bool values_finite = + bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) && PX4_ISFINITE(set_position_target_local_ned.y) && PX4_ISFINITE(set_position_target_local_ned.z) && @@ -658,7 +658,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if ((mavlink_system.sysid == set_position_target_local_ned.target_system || set_position_target_local_ned.target_system == 0) && (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0) && + set_position_target_local_ned.target_component == 0) && values_finite) { /* convert mavlink type (local, NED) to uORB offboard control struct */ @@ -675,6 +675,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t 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); bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); + bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); offboard_control_mode.timestamp = hrt_absolute_time(); @@ -717,10 +718,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (is_takeoff_sp) { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - } else if(is_land_sp) { + + } else if (is_land_sp) { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; - } else if(is_loiter_sp) { + + } else if (is_loiter_sp) { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + } else if (is_idle_sp) { + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + } else { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; } @@ -806,7 +813,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - bool values_finite = + bool values_finite = PX4_ISFINITE(set_actuator_control_target.controls[0]) && PX4_ISFINITE(set_actuator_control_target.controls[1]) && PX4_ISFINITE(set_actuator_control_target.controls[2]) &&