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

Copter: add fourth yaw behaviour, look-at-home 

Added get_wp_yaw_mode to remove duplication of checks of the
WP_YAW_BEHAVIOR parameter
This commit is contained in:
Randy Mackay 2013-04-20 15:36:24 +09:00
parent fb6b736c78
commit a53e5f747b
7 changed files with 44 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/APM_Config.h
View File

@ -42,8 +42,8 @@
* CH7_CAMERA_TRIGGER
*/
// Inertia based contollers
#define RTL_YAW YAW_HOLD
// uncomment the line below to disable control of yaw during missions (or set YAW_BEHAVIOR parameter to 0)
// #define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_NONE
//#define MOTORS_JD880
//#define MOTORS_JD850

27
ArduCopter/ArduCopter.pde
View File

@ -1541,6 +1541,33 @@ void update_yaw_mode(void)
}
}
// get yaw mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_wp_yaw_mode(bool rtl)
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
return YAW_LOOK_AT_NEXT_WP;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if( rtl ) {
return YAW_HOLD;
}else{
return YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return YAW_LOOK_AHEAD;
break;
default:
return YAW_HOLD;
break;
}
}
// set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required
bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
{

2
ArduCopter/Parameters.pde
View File

@ -187,7 +187,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Description: Determines how the autopilot controls the yaw during missions and RTL
// @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL
// @User: Advanced
GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP),
GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT),
// @Param: WP_TOTAL
// @DisplayName: Waypoint Total

15
ArduCopter/commands_logic.pde
View File

@ -278,10 +278,8 @@ static void do_nav_wp()
// this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1;
// reset control of yaw to default
if( g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP || g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL) {
set_yaw_mode(AUTO_YAW);
}
// set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter
set_yaw_mode(get_wp_yaw_mode(false));
}
// do_land - initiate landing procedure
@ -515,10 +513,8 @@ static bool verify_RTL()
// advance to next rtl state
rtl_state = RTL_STATE_INITIAL_CLIMB;
}else{
// point nose towards home
if(g.wp_yaw_behavior == WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP) {
set_yaw_mode(RTL_YAW);
}
// point nose towards home (maybe)
set_yaw_mode(get_wp_yaw_mode(true));
// Set wp navigation target to above home
wp_nav.set_destination(Vector3f(0,0,get_RTL_alt()));
@ -537,8 +533,7 @@ static bool verify_RTL()
wp_nav.set_destination(Vector3f(0,0,get_RTL_alt()));
// set yaw mode
// To-Do: make this user configurable whether RTL points towards home or not
set_yaw_mode(RTL_YAW);
set_yaw_mode(YAW_HOLD);
// advance to next rtl state
rtl_state = RTL_STATE_RETURNING_HOME;

17
ArduCopter/config.h
View File

@ -115,7 +115,7 @@
// Bulk defines for TradHeli
#if FRAME_CONFIG == HELI_FRAME
# define RC_FAST_SPEED 125
# define RTL_YAW YAW_LOOK_AT_HOME
# define WP_YAW_BEHAVIOR_DEFAULT YAW_LOOK_AT_HOME
# define RATE_INTEGRATOR_LEAK_RATE 0.02f
# define RATE_ROLL_D 0
# define RATE_PITCH_D 0
@ -579,8 +579,9 @@
#endif
// AUTO Mode
#ifndef AUTO_YAW
# define AUTO_YAW YAW_LOOK_AT_NEXT_WP
// Note: Auto mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
#ifndef WP_YAW_BEHAVIOR_DEFAULT
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP
#endif
#ifndef AUTO_RP
@ -613,10 +614,7 @@
#endif
// Guided Mode
#ifndef GUIDED_YAW
# define GUIDED_YAW YAW_LOOK_AT_NEXT_WP
#endif
// Note: Guided mode yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
#ifndef GUIDED_RP
# define GUIDED_RP ROLL_PITCH_AUTO
#endif
@ -665,10 +663,7 @@
// RTL Mode
#ifndef RTL_YAW
# define RTL_YAW YAW_LOOK_AT_NEXT_WP
#endif
// Note: RTL Yaw behaviour is controlled by WP_YAW_BEHAVIOR parameter
#ifndef RTL_RP
# define RTL_RP ROLL_PITCH_AUTO
#endif

1
ArduCopter/defines.h
View File

@ -192,6 +192,7 @@
#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
// TOY mixing options
#define TOY_LOOKUP_TABLE 0

4
ArduCopter/system.pde
View File

@ -410,7 +410,7 @@ static void set_mode(uint8_t mode)
case AUTO:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(AUTO_YAW);
set_yaw_mode(YAW_HOLD); // yaw mode will be set by mission command
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
@ -449,7 +449,7 @@ static void set_mode(uint8_t mode)
case GUIDED:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(GUIDED_YAW);
set_yaw_mode(get_wp_yaw_mode(false));
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
set_nav_mode(GUIDED_NAV);