From d541fefab436ab3f25c86575a9b66fbf4d57f2ba Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 27 Mar 2014 16:05:49 +0900
Subject: [PATCH] Copter: add do_spline_wp and verify_spline_wp functions

---
 ArduCopter/commands_logic.pde | 74 +++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index d11d6de220..e38cb0582b 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -7,6 +7,7 @@ static void do_land(const AP_Mission::Mission_Command& cmd);
 static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
 static void do_circle(const AP_Mission::Mission_Command& cmd);
 static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
+static void do_spline_wp(const AP_Mission::Mission_Command& cmd);
 static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
 static void do_within_distance(const AP_Mission::Mission_Command& cmd);
 static void do_change_alt(const AP_Mission::Mission_Command& cmd);
@@ -15,6 +16,7 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd);
 static void do_set_home(const AP_Mission::Mission_Command& cmd);
 static void do_roi(const AP_Mission::Mission_Command& cmd);
 static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 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);
 
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
@@ -58,6 +60,10 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         do_RTL();
         break;
 
+    case MAV_CMD_NAV_SPLINE_WAYPOINT:           // 82  Navigate to Waypoint using spline
+        do_spline_wp(cmd);
+        break;
+
     //
     // conditional commands
     //
@@ -188,6 +194,10 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
         return verify_RTL();
         break;
 
+    case MAV_CMD_NAV_SPLINE_WAYPOINT:
+        return verify_spline_wp(cmd);
+        break;
+
     ///
     /// conditional commands
     ///
@@ -420,6 +430,48 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
     loiter_time_max = cmd.p1;     // units are (seconds)
 }
 
+// do_spline_wp - initiate move to next waypoint
+static void do_spline_wp(const AP_Mission::Mission_Command& cmd)
+{
+    Vector3f local_pos = pv_location_to_vector(cmd.content.location);
+
+    // this will be used to remember the time in millis after we reach or pass the WP.
+    loiter_time = 0;
+    // this is the delay, stored in seconds
+    loiter_time_max = abs(cmd.p1);
+
+    // determine segment start and end type
+    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
+
+    // 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?
+    uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
+    if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
+        if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
+            if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
+                stopped_at_start = false;
+            }
+        }
+    }
+
+    // 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 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;
+        }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);
+        }
+    }
+
+    // set spline navigation target
+    auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_spline_destination);
+}
+
 /********************************************************************************/
 //	Verify Nav (Must) commands
 /********************************************************************************/
@@ -529,6 +581,28 @@ static bool verify_RTL()
     return (rtl_state_complete && (rtl_state == FinalDescent || rtl_state == Land));
 }
 
+// verify_spline_wp - check if we have reached the next way point using spline
+static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd)
+{
+    // check if we have reached the waypoint
+    if( !wp_nav.reached_wp_destination() ) {
+        return false;
+    }
+
+    // start timer if necessary
+    if(loiter_time == 0) {
+        loiter_time = millis();
+    }
+
+    // check if timer has run out
+    if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
+        gcs_send_text_fmt(PSTR("Reached Command #%i"),cmd.index);
+        return true;
+    }else{
+        return false;
+    }
+}
+
 /********************************************************************************/
 //	Condition (May) commands
 /********************************************************************************/