From 7d5d0d12a283b330ed529b053ca9e0b4e8f117be Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 29 Mar 2014 17:52:38 +0900 Subject: [PATCH] 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 --- ArduCopter/commands_logic.pde | 9 +++++---- ArduCopter/control_auto.pde | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 49b0aee354..33b7ab7de7 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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); } /********************************************************************************/ diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index a17a483c62..3df17154f3 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -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