mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-09 01:13:57 -04:00
was adding throttle cruise twice.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1527 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
f7f0fdf945
commit
28ee251978
@ -28,6 +28,7 @@ void calc_nav_throttle()
|
|||||||
|
|
||||||
// limit output of throttle control
|
// limit output of throttle control
|
||||||
t_out = throttle_cruise + constrain(t_out, -50, 100);
|
t_out = throttle_cruise + constrain(t_out, -50, 100);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// SONAR
|
// SONAR
|
||||||
t_out = pid_sonar_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
t_out = pid_sonar_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0);
|
||||||
@ -36,7 +37,7 @@ void calc_nav_throttle()
|
|||||||
t_out = throttle_cruise + constrain(t_out, -60, 100);
|
t_out = throttle_cruise + constrain(t_out, -60, 100);
|
||||||
}
|
}
|
||||||
|
|
||||||
nav_throttle = (float)(throttle_cruise + t_out) * angle_boost();
|
nav_throttle = (float)t_out * angle_boost();
|
||||||
}
|
}
|
||||||
|
|
||||||
float angle_boost()
|
float angle_boost()
|
||||||
|
Loading…
Reference in New Issue
Block a user