Copter: add fourth yaw behaviour, look-at-home

Added get_wp_yaw_mode to remove duplication of checks of the
WP_YAW_BEHAVIOR parameter

ArduCopter/APM_Config.h

@@ -42,8 +42,8 @@
  *  CH7_CAMERA_TRIGGER
  */
 
-// Inertia based contollers
+// uncomment the line below to disable control of yaw during missions (or set YAW_BEHAVIOR parameter to 0)
-#define RTL_YAW YAW_HOLD
+// #define WP_YAW_BEHAVIOR_DEFAULT  WP_YAW_BEHAVIOR_NONE
 
 //#define MOTORS_JD880
 //#define MOTORS_JD850

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)
 {

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

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
+    // set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter
-    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(get_wp_yaw_mode(false));
-        set_yaw_mode(AUTO_YAW);
-    }
 }
 
 // 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
+                // point nose towards home (maybe)
-                if(g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP) {
+                set_yaw_mode(get_wp_yaw_mode(true));
-                    set_yaw_mode(RTL_YAW);
-                }
 
                 // 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(YAW_HOLD);
-                set_yaw_mode(RTL_YAW);
 
                 // advance to next rtl state
                 rtl_state = RTL_STATE_RETURNING_HOME;

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
+// Note: Auto mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
- # define AUTO_YAW                  YAW_LOOK_AT_NEXT_WP
+#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
+// Note: Guided mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
- # define GUIDED_YAW                YAW_LOOK_AT_NEXT_WP
-#endif
-
 #ifndef GUIDED_RP
  # define GUIDED_RP                 ROLL_PITCH_AUTO
 #endif
@@ -665,10 +663,7 @@
 
 
 // RTL Mode
-#ifndef RTL_YAW
+// Note: RTL Yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
- # define RTL_YAW                   YAW_LOOK_AT_NEXT_WP
-#endif
-
 #ifndef RTL_RP
  # define RTL_RP                    ROLL_PITCH_AUTO
 #endif

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

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);