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))));