Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ACM: Creating Yaw_Look_Ahead yaw mode. 

This function is not fully tested yet.
This commit is contained in:
Robert Lefebvre 2012-11-26 21:36:32 -05:00
parent c55e6d3e6a
commit c78ecc4fd1
4 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -17,7 +17,7 @@
//#define DMP_ENABLED ENABLED //#define DMP_ENABLED ENABLED
//#define SECONDARY_DMP_ENABLED ENABLED // allows running DMP in parallel with DCM for testing purposes //#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: * options:
* QUAD_FRAME * QUAD_FRAME

13
ArduCopter/ArduCopter.pde
View File

@ -798,6 +798,8 @@ static int8_t alt_change_flag;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot. // The Commanded Yaw from the autopilot.
static int32_t nav_yaw; 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 // A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw; static int32_t auto_yaw;
static uint8_t yaw_timer; static uint8_t yaw_timer;
@ -1520,6 +1522,17 @@ void update_yaw_mode(void)
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw); get_stabilize_yaw(nav_yaw);
break; 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;
} }
} }

4
ArduCopter/config.h
View File

@ -715,6 +715,10 @@
# define STABILIZE_YAW_IMAX 8.0 // degrees * 100 # define STABILIZE_YAW_IMAX 8.0 // degrees * 100
#endif #endif
#ifndef YAW_LOOK_AHEAD_RATE
# define YAW_LOOK_AHEAD_RATE 1000 // dimensionless, smaller number means stronger effect
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Stabilize Rate Control // Stabilize Rate Control

1
ArduCopter/defines.h
View File

@ -18,6 +18,7 @@
#define YAW_AUTO 2 #define YAW_AUTO 2
#define YAW_LOOK_AT_HOME 3 #define YAW_LOOK_AT_HOME 3
#define YAW_TOY 4 // THOR This is the Yaw mode #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 #define ROLL_PITCH_STABLE 0