From fdc0ec837b11176a376b8fae0fe5b447394b1de3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 23 May 2014 14:29:08 +0900 Subject: [PATCH] Copter: add nav_guided suport to Auto mode --- ArduCopter/APM_Config.h | 1 + ArduCopter/ArduCopter.pde | 10 +++++ ArduCopter/commands_logic.pde | 73 +++++++++++++++++++++++++++++++++++ ArduCopter/config.h | 6 +++ ArduCopter/control_auto.pde | 25 ++++++++++++ ArduCopter/defines.h | 3 +- 6 files changed, 117 insertions(+), 1 deletion(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 28b96b7b9d..88a4df5c23 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -38,6 +38,7 @@ //#define OPTFLOW ENABLED // enable optical flow sensor and OF_LOITER flight mode at a cost of 5K of flash space //#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define EPM_ENABLED ENABLED // enable epm cargo gripper costs 500bytes of flash +//#define NAV_GUIDED ENABLED // enable external navigation computer to control vehicle through MAV_CMD_NAV_GUIDED mission commands // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4fb15671d9..b36860a42f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -744,6 +744,16 @@ static AP_EPM epm; static AP_Parachute parachute(relay); #endif +//////////////////////////////////////////////////////////////////////////////// +// Nav Guided - allows external computer to control the vehicle during missions +//////////////////////////////////////////////////////////////////////////////// +#if NAV_GUIDED == ENABLED +static struct { + uint32_t start_time; // system time in milliseconds that control was handed to the external computer + Vector3f start_position; // vehicle position when control was ahnded to the external computer +} nav_guided; +#endif + //////////////////////////////////////////////////////////////////////////////// // function definitions to keep compiler from complaining about undeclared functions //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index f2bfade05b..b6c4dc42d6 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -8,6 +8,9 @@ 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); +#if NAV_GUIDED == ENABLED +static void do_nav_guided(const AP_Mission::Mission_Command& cmd); +#endif 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); @@ -21,6 +24,9 @@ static void do_parachute(const AP_Mission::Mission_Command& cmd); static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); static bool verify_circle(const AP_Mission::Mission_Command& cmd); static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); +#if NAV_GUIDED == ENABLED +static bool verify_nav_guided(const AP_Mission::Mission_Command& cmd); +#endif 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 @@ -68,6 +74,12 @@ static bool start_command(const AP_Mission::Mission_Command& cmd) do_spline_wp(cmd); break; +#if NAV_GUIDED == ENABLED + case MAV_CMD_NAV_GUIDED: // 90 accept navigation commands from external nav computer + do_nav_guided(cmd); + break; +#endif + // // conditional commands // @@ -208,6 +220,12 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) return verify_spline_wp(cmd); break; +#if NAV_GUIDED == ENABLED + case MAV_CMD_NAV_GUIDED: + return verify_nav_guided(cmd); + break; +#endif + /// /// conditional commands /// @@ -483,6 +501,22 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd) auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination); } +#if NAV_GUIDED == ENABLED +// do_nav_guided - initiate accepting commands from exernal nav computer +static void do_nav_guided(const AP_Mission::Mission_Command& cmd) +{ + // record start time so it can be compared vs timeout + nav_guided.start_time = millis(); + + // record start position so it can be compared vs horizontal limit + nav_guided.start_position = inertial_nav.get_position(); + + // set spline navigation target + auto_nav_guided_start(); +} +#endif // NAV_GUIDED + + #if PARACHUTE == ENABLED // do_parachute - configure or release parachute static void do_parachute(const AP_Mission::Mission_Command& cmd) @@ -660,6 +694,42 @@ static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd) } } +#if NAV_GUIDED == ENABLED +// verify_nav_guided - check if we have breached any limits +static bool verify_nav_guided(const AP_Mission::Mission_Command& cmd) +{ + // check if we have passed the timeout + if ((cmd.p1 > 0) && ((millis() - nav_guided.start_time) / 1000 >= cmd.p1)) { + return true; + } + + // get current location + const Vector3f& curr_pos = inertial_nav.get_position(); + + // check if we have gone below min alt + if (cmd.content.nav_guided.alt_min != 0 && (curr_pos.z / 100) < cmd.content.nav_guided.alt_min) { + return true; + } + + // check if we have gone above max alt + if (cmd.content.nav_guided.alt_max != 0 && (curr_pos.z / 100) > cmd.content.nav_guided.alt_max) { + return true; + } + + // check if we have gone beyond horizontal limit + if (cmd.content.nav_guided.horiz_max != 0) { + float horiz_move = pv_get_horizontal_distance_cm(nav_guided.start_position, curr_pos) / 100; + if (horiz_move > cmd.content.nav_guided.horiz_max) { + return true; + } + } + + // if we got here we should continue with the external nav controls + return false; +} +#endif // NAV_GUIDED + + /********************************************************************************/ // Condition (May) commands /********************************************************************************/ @@ -690,6 +760,9 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd) case Auto_Circle: // move circle altitude up to target (we will need to store this target in circle class) break; + case Auto_NavGuided: + // ignore altitude + break; } } // To-Do: store desired altitude in a variable so that it can be verified later diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a715189458..a28063fc53 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -406,6 +406,12 @@ # define PARACHUTE ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Nav-Guided - allows external nav computer to control vehicle +#ifndef NAV_GUIDED + # define NAV_GUIDED DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 3019395a66..2dd9fb5226 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -70,6 +70,12 @@ static void auto_run() case Auto_Spline: auto_spline_run(); break; + +#if NAV_GUIDED == ENABLED + case Auto_NavGuided: + auto_nav_guided_run(); + break; +#endif } } @@ -364,6 +370,25 @@ void auto_circle_run() attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); } +#if NAV_GUIDED == ENABLED +// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode +void auto_nav_guided_start() +{ + auto_mode = Auto_NavGuided; + + // call regular guided flight mode initialisation + guided_init(true); +} + +// auto_nav_guided_run - allows control by external navigation controller +// called by auto_run at 100hz or more +void auto_nav_guided_run() +{ + // call regular guided flight mode run function + guided_run(); +} +#endif // NAV_GUIDED + // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL uint8_t get_default_auto_yaw_mode(bool rtl) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 959b1beafa..e5b11e6ab7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -181,7 +181,8 @@ enum AutoMode { Auto_RTL, Auto_CircleMoveToEdge, Auto_Circle, - Auto_Spline + Auto_Spline, + Auto_NavGuided }; // RTL states