diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-07 22:29:45 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-07 22:29:45 +0400 |
commit | 5397f13b500413134a9d63bc2f6440f5e61e8435 (patch) | |
tree | a40e79106abb00ed4db5f4b3351c9ff19b8822b8 /src/modules/mavlink | |
parent | 3c027a8e4d5e5e484a37f1026b6fa7835176426f (diff) | |
parent | 264ef47197432d2cc1372cabf93c3bd7a52df0aa (diff) | |
download | px4-firmware-5397f13b500413134a9d63bc2f6440f5e61e8435.tar.gz px4-firmware-5397f13b500413134a9d63bc2f6440f5e61e8435.tar.bz2 px4-firmware-5397f13b500413134a9d63bc2f6440f5e61e8435.zip |
Merge branch 'master' into hil_fixes
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/missionlib.c | 8 |
1 files changed, 4 insertions, 4 deletions
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index bb857dc69..124b3b2ae 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F); set_special_fields(wpm->waypoints[last_setpoint_index].param1, wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param3, @@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F); set_special_fields(wpm->waypoints[next_setpoint_index].param1, wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param3, @@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ @@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { |