mirror of https://github.com/ArduPilot/ardupilot
ACM: Creating Yaw_Look_Ahead yaw mode.
This function is not fully tested yet.
This commit is contained in:
parent
c55e6d3e6a
commit
c78ecc4fd1
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue