mirror of https://github.com/ArduPilot/ardupilot
Added Scaling to yaw response on Auto, RTL, and Guided modes
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2837 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
35bf288abd
commit
bdcd0e2e37
|
@ -1027,7 +1027,7 @@ void update_current_flight_mode(void)
|
|||
}
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, .5);
|
||||
break;
|
||||
}
|
||||
|
||||
|
@ -1063,7 +1063,7 @@ void update_current_flight_mode(void)
|
|||
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
|
||||
|
||||
//Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out);
|
||||
break;
|
||||
|
@ -1140,7 +1140,7 @@ void update_current_flight_mode(void)
|
|||
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
|
||||
//Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew);
|
||||
|
||||
break;
|
||||
|
@ -1164,7 +1164,7 @@ void update_current_flight_mode(void)
|
|||
}
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
|
@ -1185,7 +1185,7 @@ void update_current_flight_mode(void)
|
|||
}
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.5);
|
||||
break;
|
||||
|
||||
case LOITER:
|
||||
|
@ -1211,7 +1211,7 @@ void update_current_flight_mode(void)
|
|||
}
|
||||
|
||||
// Yaw control
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
@ -50,7 +50,7 @@ get_stabilize_pitch(long target_angle)
|
|||
}
|
||||
|
||||
int
|
||||
get_stabilize_yaw(long target_angle)
|
||||
get_stabilize_yaw(long target_angle, float scaler)
|
||||
{
|
||||
long error;
|
||||
long rate;
|
||||
|
@ -62,7 +62,7 @@ get_stabilize_yaw(long target_angle)
|
|||
error = constrain(error, -4500, 4500);
|
||||
|
||||
// desired Rate:
|
||||
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0);
|
||||
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler);
|
||||
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
|
||||
|
||||
// Rate P:
|
||||
|
|
Loading…
Reference in New Issue