From b74fe10aa9c8340eec9084442233f8d1d7970fb6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 25 Jan 2013 15:57:55 +0900 Subject: [PATCH] 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 | 79 +++++++++++++++++++++++++++++++++++-------- ArduCopter/system.pde | 33 +++++++++--------- 2 files changed, 81 insertions(+), 31 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 34b2fa286e..9bead35fc0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -608,9 +608,23 @@ #define EARTH_FRAME 0 #define BODY_FRAME 1 -// Stabilize Mode -#ifndef STABILIZE_THROTTLE - # define STABILIZE_THROTTLE THROTTLE_MANUAL_TILT_COMPENSATED +// Flight mode roll, pitch, yaw, throttle and navigation definitions + +// 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 CIRCLE_YAW - # 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 +#ifndef GUIDED_NAV + # define GUIDED_NAV NAV_WP #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 // diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 59896b2d6e..ae7fcaa9d9 100644 --- a/ArduCopter/system.pde +++ b/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_roll_pitch_mode(ROLL_PITCH_ACRO); - set_throttle_mode(THROTTLE_MANUAL); - set_nav_mode(NAV_NONE); + set_yaw_mode(ACRO_YAW); + set_roll_pitch_mode(ACRO_RP); + set_throttle_mode(ACRO_THR); + 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(¤t_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_roll_pitch_mode(LOITER_RP); - set_throttle_mode(THROTTLE_MANUAL); + set_yaw_mode(POSITION_YAW); + set_roll_pitch_mode(POSITION_RP); + set_throttle_mode(POSITION_THR); set_next_WP(¤t_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(¤t_loc); break;