Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Guard offboard and allow specifying the position setpoint type #3405

Merged
merged 4 commits into from
Mar 21, 2016
Merged

Guard offboard and allow specifying the position setpoint type #3405

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b1ee89b
add nan protection for position target local
AndreasAntener Nov 8, 2015
46ad873
add nan protection to attitude and actuator targets
AndreasAntener Nov 8, 2015
664b0ee
read setpoint type from type mask in offboard
AndreasAntener Nov 15, 2015
f3a1abf
added support for SP type idle
AndreasAntener Nov 26, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 62 additions & 7 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -642,11 +642,24 @@ 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);

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would argue that only the setpoints which are actually to be used (using the ignore mask) need to be finite.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well the others are going to be 0 if not set, if they are not finite something is wrong I'd say.

/* 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);
Expand All @@ -658,6 +671,12 @@ 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);
bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);

offboard_control_mode.timestamp = hrt_absolute_time();

if (_offboard_control_mode_pub == nullptr) {
Expand Down Expand Up @@ -696,7 +715,22 @@ 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 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;
}

/* set the local pos values */
if (!offboard_control_mode.ignore_position) {
Expand Down Expand Up @@ -733,7 +767,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;

Expand All @@ -742,7 +776,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;

Expand All @@ -751,7 +785,6 @@ 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);
Expand Down Expand Up @@ -780,10 +813,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;
Expand Down Expand Up @@ -873,11 +917,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));
Expand Down