mirror of https://github.com/ArduPilot/ardupilot
Toy Mode: Yaw performance update.
This commit is contained in:
parent
50d1ff56c5
commit
b2d932136a
|
@ -708,9 +708,26 @@ void roll_pitch_toy()
|
||||||
// Yaw control - Yaw is always available, and will NOT exit the
|
// Yaw control - Yaw is always available, and will NOT exit the
|
||||||
// user from Loiter mode
|
// user from Loiter mode
|
||||||
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
|
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
|
||||||
nav_yaw += yaw_rate / 100;
|
//nav_yaw += yaw_rate / 100;
|
||||||
nav_yaw = wrap_360(nav_yaw);
|
//nav_yaw = wrap_360(nav_yaw);
|
||||||
|
//g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||||
|
|
||||||
|
if(g.rc_1.control_in != 0){
|
||||||
|
g.rc_4.servo_out = get_acro_yaw(yaw_rate);
|
||||||
|
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(yaw_rate, g.rc_3.control_in);
|
||||||
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
if(manual_control){
|
if(manual_control){
|
||||||
// user is in control: reset count-up timer
|
// user is in control: reset count-up timer
|
||||||
|
|
Loading…
Reference in New Issue