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

Merge pull request #537 from PX4/wp_yaw_fix 

missionlib: waypoint yaw fixed
This commit is contained in:
Thomas Gubler 2013-11-28 09:35:01 -08:00
parent f1fece2bb6 69ed7cf91f
commit 95af4bf87c
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/mavlink/missionlib.c
View File

@ -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.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false; 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, set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3, 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.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false; 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, set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3, 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.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z; sp.altitude = param7_alt_z;
sp.altitude_is_relative = true; 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); set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */ /* 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.x = param5_lat_x;
sp.y = param6_lon_y; sp.y = param6_lon_y;
sp.z = param7_alt_z; 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 */ /* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) { if (local_position_setpoint_pub < 0) {