diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 98c1b9cd7a..2acdf412fc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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);