CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-30 22:59:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

simple_offboard: comment out incorrect yaw calculation if yaw=nan

Browse Source
This commit is contained in:
Oleg Kalachev
2019-03-27 08:11:54 +03:00
parent b6c493513c
commit b53bf19c8d
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
clever/src/simple_offboard.cpp
View File

@@ -528,12 +528,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
nav_speed = speed;
}
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
if (std::isnan(yaw) && yaw_rate == 0) {
// keep yaw unchanged
yaw = tf2::getYaw(local_position.pose.orientation);
}
}
// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
// if (std::isnan(yaw) && yaw_rate == 0) {
// // keep yaw unchanged
// // TODO: this is incorrect, because we need yaw in desired frame
// yaw = tf2::getYaw(local_position.pose.orientation);
// }
// }
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
// destination point and/or yaw