fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 01:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support for dynamic turn radii

Browse Source
This commit is contained in:
Lorenz Meier
2013-10-08 09:15:40 +02:00
parent 5bc7d7c00f
commit d59cdf6fcc
7 changed files with 86 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/missionlib.c
+25 -8
View File
@@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
sp->param2 = param2;
sp->param3 = param3;
sp->param4 = param4;
/* define the turn distance */
float orbit = 15.0f;
if (command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = param2;
} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
command == (int)MAV_CMD_NAV_LOITER_TIME ||
command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = param3;
} else {
// XXX set default orbit via param
// 15 initialized above
}
sp->turn_distance_xy = orbit;
sp->turn_distance_z = orbit;
}
/**
@@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
int last_setpoint_index = -1;
bool last_setpoint_valid = false;
/* at first waypoint, but cycled once through mission */
if (index == 0 && last_waypoint_index > 0) {
last_setpoint_index = last_waypoint_index;
} else {
if (index > 0) {
last_setpoint_index = index - 1;
}
@@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
int next_setpoint_index = -1;
bool next_setpoint_valid = false;
/* at last waypoint, try to re-loop through mission as default */
if (index == (wpm->size - 1) && wpm->size > 1) {
next_setpoint_index = 0;
} else if (wpm->size > 1) {
/* next waypoint */
if (wpm->size > 1) {
next_setpoint_index = index + 1;
}