Copter: set each flight mode's nav_mode

Allow ACRO flight mode's roll, pitch, yaw, throttle and nav mode to be
overwritten

ArduCopter/config.h

@@ -608,9 +608,23 @@
 #define EARTH_FRAME     0
 #define BODY_FRAME      1
 
-// Stabilize Mode
+// Flight mode roll, pitch, yaw, throttle and navigation definitions
-#ifndef STABILIZE_THROTTLE
+
- # define STABILIZE_THROTTLE		THROTTLE_MANUAL_TILT_COMPENSATED
+// Acro Mode
+#ifndef ACRO_YAW
+ # define ACRO_YAW           	    YAW_ACRO
+#endif
+
+#ifndef ACRO_RP
+ # define ACRO_RP            	    ROLL_PITCH_ACRO
+#endif
+
+#ifndef ACRO_THR
+ # define ACRO_THR           	    THROTTLE_MANUAL
+#endif
+
+#ifndef ACRO_NAV
+ # define ACRO_NAV           	    NAV_NONE
 #endif
 
 // Alt Hold Mode
@@ -626,6 +640,10 @@
  # define ALT_HOLD_THR           	THROTTLE_HOLD
 #endif
 
+#ifndef ALT_HOLD_NAV
+ # define ALT_HOLD_NAV           	NAV_NONE
+#endif
+
 // AUTO Mode
 #ifndef AUTO_YAW
  # define AUTO_YAW                  YAW_LOOK_AT_NEXT_WP
@@ -639,6 +657,23 @@
  # define AUTO_THR                  THROTTLE_AUTO
 #endif
 
+// CIRCLE Mode
+#ifndef CIRCLE_YAW
+ # define CIRCLE_YAW             	YAW_LOOK_AT_LOCATION
+#endif
+
+#ifndef CIRCLE_RP
+ # define CIRCLE_RP                 ROLL_PITCH_AUTO
+#endif
+
+#ifndef CIRCLE_THR
+ # define CIRCLE_THR                THROTTLE_HOLD
+#endif
+
+#ifndef CIRCLE_NAV
+ # define CIRCLE_NAV           	    NAV_CIRCLE
+#endif
+
 // Guided Mode
 #ifndef GUIDED_YAW
  # define GUIDED_YAW                YAW_LOOK_AT_NEXT_WP
@@ -652,17 +687,8 @@
  # define GUIDED_THR                THROTTLE_AUTO
 #endif
 
-// CIRCLE Mode
+#ifndef GUIDED_NAV
-#ifndef CIRCLE_YAW
+ # define GUIDED_NAV           	    NAV_WP
- # define CIRCLE_YAW             	YAW_LOOK_AT_NEXT_WP
-#endif
-
-#ifndef CIRCLE_RP
- # define CIRCLE_RP                 ROLL_PITCH_AUTO
-#endif
-
-#ifndef CIRCLE_THR
- # define CIRCLE_THR                THROTTLE_HOLD
 #endif
 
 // LOITER Mode
@@ -678,6 +704,27 @@
  # define LOITER_THR                THROTTLE_HOLD
 #endif
 
+#ifndef LOITER_NAV
+ # define LOITER_NAV                NAV_LOITER
+#endif
+
+// POSITION Mode
+#ifndef POSITION_YAW
+ # define POSITION_YAW             	YAW_HOLD
+#endif
+
+#ifndef POSITION_RP
+ # define POSITION_RP               ROLL_PITCH_AUTO
+#endif
+
+#ifndef POSITION_THR
+ # define POSITION_THR              THROTTLE_HOLD
+#endif
+
+#ifndef POSITION_NAV
+ # define POSITION_NAV              NAV_LOITER
+#endif
+
 
 // RTL Mode
 #ifndef RTL_YAW
@@ -732,6 +779,10 @@
  # define OF_LOITER_THR             THROTTLE_HOLD
 #endif
 
+#ifndef OF_LOITER_NAV
+ # define OF_LOITER_NAV             NAV_NONE
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Attitude Control
 //

ArduCopter/system.pde

@@ -402,10 +402,10 @@ static void set_mode(uint8_t mode)
     case ACRO:
     	ap.manual_throttle = true;
     	ap.manual_attitude = true;
-        set_yaw_mode(YAW_ACRO);
+        set_yaw_mode(ACRO_YAW);
-        set_roll_pitch_mode(ROLL_PITCH_ACRO);
+        set_roll_pitch_mode(ACRO_RP);
-        set_throttle_mode(THROTTLE_MANUAL);
+        set_throttle_mode(ACRO_THR);
-        set_nav_mode(NAV_NONE);
+        set_nav_mode(ACRO_NAV);
         // reset acro axis targets to current attitude
 		if(g.axis_enabled){
             roll_axis 	= ahrs.roll_sensor;
@@ -419,7 +419,7 @@ static void set_mode(uint8_t mode)
     	ap.manual_attitude = true;
         set_yaw_mode(YAW_HOLD);
         set_roll_pitch_mode(ROLL_PITCH_STABLE);
-        set_throttle_mode(STABILIZE_THROTTLE);
+        set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
         set_nav_mode(NAV_NONE);
         break;
 
@@ -429,7 +429,7 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(ALT_HOLD_YAW);
         set_roll_pitch_mode(ALT_HOLD_RP);
         set_throttle_mode(ALT_HOLD_THR);
-        set_nav_mode(NAV_NONE);
+        set_nav_mode(ALT_HOLD_NAV);
         break;
 
     case AUTO:
@@ -438,7 +438,7 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(AUTO_YAW);
         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
         // loads the commands from where we left off
         init_commands();
         break;
@@ -446,13 +446,12 @@ static void set_mode(uint8_t mode)
     case CIRCLE:
     	ap.manual_throttle = false;
     	ap.manual_attitude = false;
-
         // set yaw to point to center of circle
         yaw_look_at_WP = circle_WP;
-        set_yaw_mode(YAW_LOOK_AT_LOCATION);
+        set_yaw_mode(CIRCLE_YAW);
         set_roll_pitch_mode(CIRCLE_RP);
         set_throttle_mode(CIRCLE_THR);
-        set_nav_mode(NAV_CIRCLE);
+        set_nav_mode(CIRCLE_NAV);
         break;
 
     case LOITER:
@@ -462,17 +461,17 @@ static void set_mode(uint8_t mode)
         set_roll_pitch_mode(LOITER_RP);
         set_throttle_mode(LOITER_THR);
         set_next_WP(&current_loc);
-        set_nav_mode(NAV_LOITER);
+        set_nav_mode(LOITER_NAV);
         break;
 
     case POSITION:
     	ap.manual_throttle = true;
     	ap.manual_attitude = false;
-        set_yaw_mode(YAW_HOLD);
+        set_yaw_mode(POSITION_YAW);
-        set_roll_pitch_mode(LOITER_RP);
+        set_roll_pitch_mode(POSITION_RP);
-        set_throttle_mode(THROTTLE_MANUAL);
+        set_throttle_mode(POSITION_THR);
         set_next_WP(&current_loc);
-        set_nav_mode(NAV_LOITER);
+        set_nav_mode(POSITION_NAV);
         break;
 
     case GUIDED:
@@ -481,7 +480,7 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(GUIDED_YAW);
         set_roll_pitch_mode(GUIDED_RP);
         set_throttle_mode(GUIDED_THR);
-        set_nav_mode(NAV_WP);
+        set_nav_mode(GUIDED_NAV);
         wp_verify_byte = 0;
         set_next_WP(&guided_WP);
         break;
@@ -514,7 +513,7 @@ static void set_mode(uint8_t mode)
         set_yaw_mode(OF_LOITER_YAW);
         set_roll_pitch_mode(OF_LOITER_RP);
         set_throttle_mode(OF_LOITER_THR);
-        set_nav_mode(NAV_NONE);
+        set_nav_mode(OF_LOITER_NAV);
         set_next_WP(&current_loc);
         break;