yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use explicit typemasks for offboard position setpoints (#16296)

Browse Source 
This uses enums of the POSITION_TARGET_TYPEMASK in order to separate typemasks that are part of the mavlink specs more explicitly
This commit is contained in:
JaeyoungLim
2020-11-30 22:12:06 +01:00
committed by GitHub
parent a63e8cc149
commit bd943d872c
1 changed files with 43 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -811,26 +811,41 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
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_local_ned.type_mask & 0x7);
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
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);
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_X_IGNORE
| POSITION_TARGET_TYPEMASK_Y_IGNORE
| POSITION_TARGET_TYPEMASK_Z_IGNORE));
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_Z_IGNORE);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_VX_IGNORE
| POSITION_TARGET_TYPEMASK_VY_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE));
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_AX_IGNORE
| POSITION_TARGET_TYPEMASK_AY_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_IGNORE);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
/* yaw ignore flag mapps to ignore_attitude */
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET);
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);
bool is_gliding_sp = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE));
(POSITION_TARGET_TYPEMASK_Z_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);
@@ -982,29 +997,30 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
/* convert mavlink type (local, NED) to uORB offboard control struct */
offboard_control_mode.ignore_position = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_X_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Y_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE));
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask & 0x4);
(POSITION_TARGET_TYPEMASK_X_IGNORE
| POSITION_TARGET_TYPEMASK_Y_IGNORE
| POSITION_TARGET_TYPEMASK_Z_IGNORE));
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_Z_IGNORE);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VX_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VY_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE));
(POSITION_TARGET_TYPEMASK_VX_IGNORE
| POSITION_TARGET_TYPEMASK_VY_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE));
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AX_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AY_IGNORE
| POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE));
(POSITION_TARGET_TYPEMASK_AX_IGNORE
| POSITION_TARGET_TYPEMASK_AY_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_IGNORE);
POSITION_TARGET_TYPEMASK_YAW_IGNORE);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
bool is_force_sp = (bool)(set_position_target_global_int.type_mask & (1 << 9));
bool is_force_sp = (bool)(set_position_target_global_int.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET);
offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);