diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 22a33d8517..1fa91feab2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -656,11 +656,11 @@ static int32_t baro_alt; // Each Flight mode is a unique combination of these modes // // The current desired control scheme for Yaw -static uint8_t yaw_mode; +static uint8_t yaw_mode = STABILIZE_YAW; // The current desired control scheme for roll and pitch / navigation -static uint8_t roll_pitch_mode; +static uint8_t roll_pitch_mode = STABILIZE_RP; // The current desired control scheme for altitude hold -static uint8_t throttle_mode; +static uint8_t throttle_mode = STABILIZE_THR; //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 07daf8c56e..9cf4a58443 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -532,6 +532,12 @@ // Flight mode roll, pitch, yaw, throttle and navigation definitions // Stabilize Mode +#ifndef STABILIZE_YAW + # define STABILIZE_YAW YAW_HOLD +#endif +#ifndef STABILIZE_RP + # define STABILIZE_RP ROLL_PITCH_STABLE +#endif #ifndef STABILIZE_THR # define STABILIZE_THR THROTTLE_MANUAL_TILT_COMPENSATED #endif diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 0a39a2a7dd..ede08bb38a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -352,8 +352,8 @@ static bool set_mode(uint8_t mode) case STABILIZE: success = true; - set_yaw_mode(YAW_HOLD); - set_roll_pitch_mode(ROLL_PITCH_STABLE); + set_yaw_mode(STABILIZE_YAW); + set_roll_pitch_mode(STABILIZE_RP); set_throttle_mode(STABILIZE_THR); set_nav_mode(NAV_NONE); break;