Skip to content

why are they different on /mavros/setpoint_raw/local and /mavros/setpoint_position/local about handling yaw #2047

@Tfly6

Description

@Tfly6

In /mavros/setpoint_position/local(setpoint_position.cpp)

auto q = [&] () {
	if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
		return ftf::transform_orientation_absolute_frame_aircraft_baselink(Eigen::Quaterniond(tr.rotation()));
	} else {
		return ftf::transform_orientation_enu_ned(
			ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
	}
} ();

In /mavros/setpoint_raw/local(setpoint_raw.cpp), the yaw have two transform, ned -> enu, frd -> flu ?

position = ftf::transform_frame_enu_ned(position);
velocity = ftf::transform_frame_enu_ned(velocity);
af = ftf::transform_frame_enu_ned(af);
yaw = ftf::quaternion_get_yaw(
		ftf::transform_orientation_aircraft_baselink(
			ftf::transform_orientation_ned_enu(
				ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions