diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index aa23fe53c8..375c69641d 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -42,8 +42,8 @@ * CH7_CAMERA_TRIGGER */ -// Inertia based contollers -#define RTL_YAW YAW_HOLD +// uncomment the line below to disable control of yaw during missions (or set YAW_BEHAVIOR parameter to 0) +// #define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_NONE //#define MOTORS_JD880 //#define MOTORS_JD850 diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1397a4fc00..a45b8c1b77 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1541,6 +1541,33 @@ void update_yaw_mode(void) } } +// get yaw mode based on WP_YAW_BEHAVIOR parameter +// set rtl parameter to true if this is during an RTL +uint8_t get_wp_yaw_mode(bool rtl) +{ + switch (g.wp_yaw_behavior) { + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: + return YAW_LOOK_AT_NEXT_WP; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: + if( rtl ) { + return YAW_HOLD; + }else{ + return YAW_LOOK_AT_NEXT_WP; + } + break; + + case WP_YAW_BEHAVIOR_LOOK_AHEAD: + return YAW_LOOK_AHEAD; + break; + + default: + return YAW_HOLD; + break; + } +} + // set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) { diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 20e3c51127..cc4350d31a 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -187,7 +187,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Description: Determines how the autopilot controls the yaw during missions and RTL // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL // @User: Advanced - GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP), + GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), // @Param: WP_TOTAL // @DisplayName: Waypoint Total diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index ad6300c3e3..c82c362c0e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -278,10 +278,8 @@ static void do_nav_wp() // this is the delay, stored in seconds and expanded to millis loiter_time_max = command_nav_queue.p1; - // reset control of yaw to default - if( g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP || g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL) { - set_yaw_mode(AUTO_YAW); - } + // set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter + set_yaw_mode(get_wp_yaw_mode(false)); } // do_land - initiate landing procedure @@ -515,10 +513,8 @@ static bool verify_RTL() // advance to next rtl state rtl_state = RTL_STATE_INITIAL_CLIMB; }else{ - // point nose towards home - if(g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP) { - set_yaw_mode(RTL_YAW); - } + // point nose towards home (maybe) + set_yaw_mode(get_wp_yaw_mode(true)); // Set wp navigation target to above home wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); @@ -537,8 +533,7 @@ static bool verify_RTL() wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); // set yaw mode - // To-Do: make this user configurable whether RTL points towards home or not - set_yaw_mode(RTL_YAW); + set_yaw_mode(YAW_HOLD); // advance to next rtl state rtl_state = RTL_STATE_RETURNING_HOME; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index af98d77a3b..b99849fa85 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -115,7 +115,7 @@ // Bulk defines for TradHeli #if FRAME_CONFIG == HELI_FRAME # define RC_FAST_SPEED 125 - # define RTL_YAW YAW_LOOK_AT_HOME + # define WP_YAW_BEHAVIOR_DEFAULT YAW_LOOK_AT_HOME # define RATE_INTEGRATOR_LEAK_RATE 0.02f # define RATE_ROLL_D 0 # define RATE_PITCH_D 0 @@ -579,8 +579,9 @@ #endif // AUTO Mode -#ifndef AUTO_YAW - # define AUTO_YAW YAW_LOOK_AT_NEXT_WP +// Note: Auto mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter +#ifndef WP_YAW_BEHAVIOR_DEFAULT + # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP #endif #ifndef AUTO_RP @@ -613,10 +614,7 @@ #endif // Guided Mode -#ifndef GUIDED_YAW - # define GUIDED_YAW YAW_LOOK_AT_NEXT_WP -#endif - +// Note: Guided mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter #ifndef GUIDED_RP # define GUIDED_RP ROLL_PITCH_AUTO #endif @@ -665,10 +663,7 @@ // RTL Mode -#ifndef RTL_YAW - # define RTL_YAW YAW_LOOK_AT_NEXT_WP -#endif - +// Note: RTL Yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter #ifndef RTL_RP # define RTL_RP ROLL_PITCH_AUTO #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c222c73100..be847d5b43 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -192,6 +192,7 @@ #define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last +#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers) // TOY mixing options #define TOY_LOOKUP_TABLE 0 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6063244860..aed769d8ca 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -410,7 +410,7 @@ static void set_mode(uint8_t mode) case AUTO: ap.manual_throttle = false; ap.manual_attitude = false; - set_yaw_mode(AUTO_YAW); + set_yaw_mode(YAW_HOLD); // yaw mode will be set by mission command set_roll_pitch_mode(AUTO_RP); set_throttle_mode(AUTO_THR); // we do not set nav mode for auto because it will be overwritten when first command runs @@ -449,7 +449,7 @@ static void set_mode(uint8_t mode) case GUIDED: ap.manual_throttle = false; ap.manual_attitude = false; - set_yaw_mode(GUIDED_YAW); + set_yaw_mode(get_wp_yaw_mode(false)); set_roll_pitch_mode(GUIDED_RP); set_throttle_mode(GUIDED_THR); set_nav_mode(GUIDED_NAV);