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

Jump to bottom

fixed yaw problems in setponit_raw and setponit_position when using BODY_NED and BODY_OFFSET_NED #1537

Merged
merged 1 commit into from
Feb 15, 2021
Merged

fixed yaw problems in setponit_raw and setponit_position when using BODY_NED and BODY_OFFSET_NED #1537

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
22 changes: 22 additions & 0 deletions mavros/include/mavros/frame_tf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ enum class StaticTF {
ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame
AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame
BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft
ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,//!< change orientation from being expressed in aircraft frame to baselink frame in an absolute frame of reference.
ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,//!< change orientation from being expressed in baselink frame to aircraft frame in an absolute frame of reference
};

/**
Expand Down Expand Up @@ -186,6 +188,26 @@ inline T transform_orientation_baselink_aircraft(const T &in) {
return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT);
}

/**
* @brief Transform from attitude represented WRT aircraft frame to
* attitude represented WRT base_link frame, treating aircraft frame
* as in an absolute frame of reference (local NED).
*/
template<class T>
inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) {
return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK);
}

/**
* @brief Transform from attitude represented WRT baselink frame to
* attitude represented WRT body frame, treating baselink frame
* as in an absolute frame of reference (local NED).
*/
template<class T>
inline T transform_orientation_absolute_frame_baselink_aircraft(const T &in) {
return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT);
}

/**
* @brief Transform data expressed in NED to ENU frame.
*/
Expand Down
4 changes: 4 additions & 0 deletions mavros/src/lib/ftf_frame_conversions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,10 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat
case StaticTF::AIRCRAFT_TO_BASELINK:
case StaticTF::BASELINK_TO_AIRCRAFT:
return q * AIRCRAFT_BASELINK_Q;

case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK:
case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT:
return AIRCRAFT_BASELINK_Q * q;
}
}

Expand Down
2 changes: 1 addition & 1 deletion mavros/src/plugins/setpoint_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class SetpointPositionPlugin : public plugin::PluginBase,

auto q = [&] () {
if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
return ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()));
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())));
Expand Down
9 changes: 6 additions & 3 deletions mavros/src/plugins/setpoint_raw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,16 +166,19 @@ class SetpointRawPlugin : public plugin::PluginBase,
position = ftf::transform_frame_baselink_aircraft(position);
velocity = ftf::transform_frame_baselink_aircraft(velocity);
af = ftf::transform_frame_baselink_aircraft(af);
yaw = ftf::quaternion_get_yaw(
ftf::transform_orientation_absolute_frame_aircraft_baselink(
ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)));
} else {
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(
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))));
}

Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
yaw_rate = ang_vel_ned.z();
Expand Down