diff options
| author | Thomas Gubler <thomasgubler@student.ethz.ch> | 2013-11-28 09:35:01 -0800 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@student.ethz.ch> | 2013-11-28 09:35:01 -0800 |
| commit | 95af4bf87c2ffc816c27a99c90af5d154cc9ff67 (patch) | |
| tree | c627d3db42051ff0d294bc575423a03091cb00f8 | |
| parent | f1fece2bb6fe4d40128f3f17b92c073d50cce982 (diff) | |
| parent | 69ed7cf91f9f82ce9ed7fadde5fd584e8c0e5c18 (diff) | |
| download | px4-firmware-95af4bf87c2ffc816c27a99c90af5d154cc9ff67.tar.gz px4-firmware-95af4bf87c2ffc816c27a99c90af5d154cc9ff67.tar.bz2 px4-firmware-95af4bf87c2ffc816c27a99c90af5d154cc9ff67.zip | |
Merge pull request #537 from PX4/wp_yaw_fix
missionlib: waypoint yaw fixed
| -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) { |