mirror of https://github.com/ArduPilot/ardupilot
ACM : attitude.pde - removed unused function
This commit is contained in:
parent
e12ce1d5a7
commit
132ac7d412
|
@ -350,26 +350,6 @@ get_rate_yaw(int32_t target_rate)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static int16_t
|
|
||||||
get_nav_throttle(int32_t z_error)
|
|
||||||
{
|
|
||||||
int16_t z_target_speed;
|
|
||||||
|
|
||||||
// convert to desired Rate:
|
|
||||||
z_target_speed = g.pi_alt_hold.get_p(z_error);
|
|
||||||
z_target_speed = constrain(z_target_speed, -250, 250);
|
|
||||||
|
|
||||||
// limit error to prevent I term wind up
|
|
||||||
z_error = constrain(z_error, -400, 400);
|
|
||||||
|
|
||||||
// compensates throttle setpoint error for hovering
|
|
||||||
int16_t i_hold = g.pi_alt_hold.get_i(z_error, .02);
|
|
||||||
|
|
||||||
// output control:
|
|
||||||
return get_throttle_rate(z_target_speed) + i_hold; //+ boost_p;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static int16_t
|
static int16_t
|
||||||
get_throttle_rate(int16_t z_target_speed)
|
get_throttle_rate(int16_t z_target_speed)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue