Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: spline bug fixes 

Next waypoint's location must be passed in even if it's a straight
segment.
mission.get_next_nav_cmd's start_index should be the current command +1
This commit is contained in:
Randy Mackay 2014-03-29 17:52:38 +09:00
parent e1819bb53a
commit 7d5d0d12a2
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/commands_logic.pde
View File

@ -408,7 +408,7 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd)
bool stopped_at_start = true;
AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
AP_Mission::Mission_Command temp_cmd;
Vector3f next_spline_destination; // end of next segment if it is a spline segment
Vector3f next_destination; // end of next segment
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
@ -422,18 +422,19 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd)
}
// if there is no delay at the end of this segment get next nav command
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index, temp_cmd)) {
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
// if the next nav command is a waypoint set end type to spline or straight
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
next_destination = pv_location_to_vector(temp_cmd.content.location);
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
next_spline_destination = pv_location_to_vector(temp_cmd.content.location);
next_destination = pv_location_to_vector(temp_cmd.content.location);
}
}
// set spline navigation target
auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_spline_destination);
auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination);
}
/********************************************************************************/

5
ArduCopter/control_auto.pde
View File

@ -174,12 +174,13 @@ static void auto_wp_run()
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination)
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_destination)
{
auto_mode = Auto_Spline;
// initialise wpnav
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_spline_destination);
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI