dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

multirotor_pos_control: failsafe against invalid yaw setpoint in AUTO

This commit is contained in:
Anton Babushkin 2013-11-09 14:11:39 +04:00
parent c63995e91c
commit 03162f5f0d
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);