mirror of https://github.com/ArduPilot/ardupilot
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:
parent
bea1ab8810
commit
0aabd4efe2
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue