Yaw mode:

Improved hybrid Yaw mode:
- uses rate controller rather than as fast as possible
- waits 1.5 seconds before entering hold
- bounce free
This commit is contained in:
Jason Short 2012-07-01 13:38:53 -07:00
parent bea1ab8810
commit 0aabd4efe2
2 changed files with 26 additions and 14 deletions

View File

@ -801,6 +801,8 @@ static int32_t nav_yaw;
// A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw;
// Used to manage the Yaw hold capabilities -
static bool yaw_stopped;
static uint8_t yaw_timer;
// Options include: no tracking, point at next wp, or at a target
static byte yaw_tracking = MAV_ROI_WPNEXT;
// In AP Mission scripting we have a fine level of control for Yaw
@ -1107,7 +1109,6 @@ static void medium_loop()
//update_altitude();
//#endif
alt_sensor_flag = true;
break;
// This case deals with sending high rate telemetry
@ -1484,12 +1485,22 @@ void update_yaw_mode(void)
break;
case YAW_HOLD:
// calcualte new nav_yaw offset
if (control_mode <= STABILIZE){
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
if(g.rc_4.control_in != 0){
g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in);
yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped){
g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--;
if(yaw_timer == 0){
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1);
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
}
return;
break;
case YAW_LOOK_AT_HOME:
@ -2125,9 +2136,9 @@ adjust_altitude()
manual_boost = (g.rc_3.control_in - g.throttle_min) - THROTTLE_ADJUST;
manual_boost = max(-THROTTLE_ADJUST, manual_boost);
}else if (g.rc_3.control_in >= (MAXIMUM_THROTTLE - THROTTLE_ADJUST)){
}else if (g.rc_3.control_in >= (g.throttle_max - THROTTLE_ADJUST)){
// we add 0 to 100 PWM to hover
manual_boost = g.rc_3.control_in - (MAXIMUM_THROTTLE - THROTTLE_ADJUST);
manual_boost = g.rc_3.control_in - (g.throttle_max - THROTTLE_ADJUST);
manual_boost = min(THROTTLE_ADJUST, manual_boost);
}else {

View File

@ -253,8 +253,15 @@ get_rate_yaw(int32_t target_rate)
output = p+i+d;
// constrain output
#if FRAME_CONFIG == HELI_FRAME
output = constrain(output, -4500, 4500);
#else
// output control:
int16_t yaw_limit = 1900 + abs(g.rc_4.control_in);
// constrain output
output = constrain(output, -yaw_limit, yaw_limit);
#endif
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
@ -407,22 +414,16 @@ get_nav_yaw_offset(int yaw_input, int reset)
return ahrs.yaw_sensor;
}else{
#if ALTERNATIVE_YAW_MODE == ENABLED
_yaw = nav_yaw + (yaw_input / 50);
return wrap_360(_yaw);
#else
// re-define nav_yaw if we have stick input
if(yaw_input != 0){
// set nav_yaw + or - the current location
_yaw = yaw_input + ahrs.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw);
}else{
// no stick input, lets not change nav_yaw
return nav_yaw;
}
#endif
}
}