diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 4e94f38145..386e9fe1e0 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -17,7 +17,7 @@ //#define DMP_ENABLED ENABLED //#define SECONDARY_DMP_ENABLED ENABLED // allows running DMP in parallel with DCM for testing purposes -//#define FRAME_CONFIG QUAD_FRAME +#define FRAME_CONFIG HELI_FRAME /* * options: * QUAD_FRAME diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0effee7438..e5091cde67 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -798,6 +798,8 @@ static int8_t alt_change_flag; //////////////////////////////////////////////////////////////////////////////// // The Commanded Yaw from the autopilot. static int32_t nav_yaw; +// Commanded Yaw to automatically look ahead. +static int32_t look_ahead_yaw; // A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot static int32_t auto_yaw; static uint8_t yaw_timer; @@ -1520,6 +1522,17 @@ void update_yaw_mode(void) nav_yaw = wrap_360(nav_yaw); get_stabilize_yaw(nav_yaw); break; + + case YAW_LOOK_AHEAD: + + if (g_gps->ground_speed > 300){ // Speed in cm/s. Create deadband of 3m/s. + look_ahead_yaw = (float)((g_gps->ground_speed - 300)/YAW_LOOK_AHEAD_RATE * (g_gps->ground_course - nav_yaw)); + look_ahead_yaw = constrain ((look_ahead_yaw + g.rc_4.control_in), -4500, 4500); + } else { + look_ahead_yaw = g.rc_4.control_in; + } + get_yaw_rate_stabilized_ef(look_ahead_yaw); + break; } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 45f21f25de..277d37f42f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -715,6 +715,10 @@ # define STABILIZE_YAW_IMAX 8.0 // degrees * 100 #endif +#ifndef YAW_LOOK_AHEAD_RATE + # define YAW_LOOK_AHEAD_RATE 1000 // dimensionless, smaller number means stronger effect +#endif + ////////////////////////////////////////////////////////////////////////////// // Stabilize Rate Control diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4bb0238fb7..43465df477 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,6 +18,7 @@ #define YAW_AUTO 2 #define YAW_LOOK_AT_HOME 3 #define YAW_TOY 4 // THOR This is the Yaw mode +#define YAW_LOOK_AHEAD 5 // WARNING! CODE IN DEVELOPMENT NOT PROVEN #define ROLL_PITCH_STABLE 0