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

Enable support for global position setpoints with SET_POSITION_TARGET_GLOBAL_INT #12819

Merged
merged 8 commits into from
Sep 3, 2019
Merged

Enable support for global position setpoints with SET_POSITION_TARGET_GLOBAL_INT #12819

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
5962783
Add placeholders for handling global position setpoints
Jaeyoung-Lim Aug 26, 2019
07b79e4
Add global position subscription
Jaeyoung-Lim Aug 26, 2019
c37fbf3
Update src/modules/mavlink/mavlink_receiver.cpp
Jaeyoung-Lim Aug 28, 2019
8b7af5b
Update src/modules/mavlink/mavlink_receiver.cpp
Jaeyoung-Lim Aug 28, 2019
d6a0505
Fix Ci type conversion errors
Jaeyoung-Lim Aug 28, 2019
07e944f
Stop using magic numbers
Jaeyoung-Lim Aug 28, 2019
5710446
Clean up setpoint type parsing
Jaeyoung-Lim Aug 29, 2019
fdf0b35
Merge branch 'master' into pr-global-position-setpoint
Jaeyoung-Lim Aug 29, 2019
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
174 changes: 174 additions & 0 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_set_position_target_local_ned(msg);
break;

case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
handle_message_set_position_target_global_int(msg);
break;

case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_message_set_attitude_target(msg);
break;
Expand Down Expand Up @@ -885,6 +889,176 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
}
}

void
MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t *msg)
{
mavlink_set_position_target_global_int_t set_position_target_global_int;
mavlink_msg_set_position_target_global_int_decode(msg, &set_position_target_global_int);

const bool values_finite =
PX4_ISFINITE(set_position_target_global_int.alt) &&
PX4_ISFINITE(set_position_target_global_int.vx) &&
PX4_ISFINITE(set_position_target_global_int.vy) &&
PX4_ISFINITE(set_position_target_global_int.vz) &&
PX4_ISFINITE(set_position_target_global_int.afx) &&
PX4_ISFINITE(set_position_target_global_int.afy) &&
PX4_ISFINITE(set_position_target_global_int.afz) &&
PX4_ISFINITE(set_position_target_global_int.yaw);

/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_global_int.target_system ||
set_position_target_global_int.target_system == 0) &&
(mavlink_system.compid == set_position_target_global_int.target_component ||
set_position_target_global_int.target_component == 0) &&
values_finite) {
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved

offboard_control_mode_s offboard_control_mode{};

/* convert mavlink type (local, NED) to uORB offboard control struct */
offboard_control_mode.ignore_position = (bool)(set_position_target_global_int.type_mask & 0x7);
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask & 0x4);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask & 0x38);
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask & 0x1C0);
offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask & 0x400);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask & 0x800);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask & 0x800);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask & 0x800);

/* yaw ignore flag mapps to ignore_attitude */
bool is_force_sp = (bool)(set_position_target_global_int.type_mask & (1 << 9));

bool is_takeoff_sp = (bool)(set_position_target_global_int.type_mask & 0x1000);
bool is_land_sp = (bool)(set_position_target_global_int.type_mask & 0x2000);
bool is_loiter_sp = (bool)(set_position_target_global_int.type_mask & 0x3000);
bool is_idle_sp = (bool)(set_position_target_global_int.type_mask & 0x4000);
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved

offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);

/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {

vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);

if (control_mode.flag_control_offboard_enabled) {
if (is_force_sp && offboard_control_mode.ignore_position &&
offboard_control_mode.ignore_velocity) {

PX4_WARN("force setpoint not supported");

} else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
position_setpoint_triplet_s pos_sp_triplet{};

pos_sp_triplet.timestamp = hrt_absolute_time();
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;

/* Order of statements matters. Takeoff can override loiter.
* See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */
if (is_loiter_sp) {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;

} else 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_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 */
vehicle_local_position_s local_pos{};

if (!offboard_control_mode.ignore_position && _vehicle_local_position_sub.copy(&local_pos)) {
if (!globallocalconverter_initialized()) {
globallocalconverter_init(local_pos.ref_lat, local_pos.ref_lon,
local_pos.ref_alt, local_pos.ref_timestamp);
pos_sp_triplet.current.position_valid = false;

} else {
globallocalconverter_tolocal(set_position_target_global_int.lat_int / 10000000.0f,
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
set_position_target_global_int.lon_int / 10000000.0f, set_position_target_global_int.alt,
&pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z);
pos_sp_triplet.current.position_valid = true;
}

} else {
pos_sp_triplet.current.position_valid = false;
}

/* set the local vel values */
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
if (!offboard_control_mode.ignore_velocity) {

pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_global_int.vx;
pos_sp_triplet.current.vy = set_position_target_global_int.vy;
pos_sp_triplet.current.vz = set_position_target_global_int.vz;

pos_sp_triplet.current.velocity_frame = set_position_target_global_int.coordinate_frame;

} else {
pos_sp_triplet.current.velocity_valid = false;
}

if (!offboard_control_mode.ignore_alt_hold) {
pos_sp_triplet.current.alt_valid = true;

} else {
pos_sp_triplet.current.alt_valid = false;
}

/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (!offboard_control_mode.ignore_acceleration_force) {

pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_global_int.afx;
pos_sp_triplet.current.a_y = set_position_target_global_int.afy;
pos_sp_triplet.current.a_z = set_position_target_global_int.afz;
pos_sp_triplet.current.acceleration_is_force = is_force_sp;

} else {
pos_sp_triplet.current.acceleration_valid = false;
}

/* set the yaw sp value */
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
if (!offboard_control_mode.ignore_attitude) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_global_int.yaw;

} else {
pos_sp_triplet.current.yaw_valid = false;
}

/* set the yawrate sp value */
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_global_int.yaw_rate;

} else {
pos_sp_triplet.current.yawspeed_valid = false;
}

//XXX handle global pos setpoints (different MAV frames)
Jaeyoung-Lim marked this conversation as resolved.
Show resolved Hide resolved
_pos_sp_triplet_pub.publish(pos_sp_triplet);
}
}
}
}
}

void
MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
{
Expand Down
3 changes: 2 additions & 1 deletion src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ class MavlinkReceiver : public ModuleParams
void handle_message_set_attitude_target(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_set_position_target_global_int(mavlink_message_t *msg);
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
void handle_message_utm_global_position(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
Expand Down Expand Up @@ -260,7 +261,7 @@ class MavlinkReceiver : public ModuleParams
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};

uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};

static constexpr unsigned int MOM_SWITCH_COUNT{8};
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
Expand Down