mirror of https://github.com/ArduPilot/ardupilot
Added Acro_P
This commit is contained in:
parent
574b86b02e
commit
fcb24ee17d
|
@ -83,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle)
|
||||||
static int
|
static int
|
||||||
get_acro_roll(int32_t target_rate)
|
get_acro_roll(int32_t target_rate)
|
||||||
{
|
{
|
||||||
target_rate = target_rate * g.pi_stabilize_roll.kP();
|
target_rate = target_rate * g.acro_P;
|
||||||
target_rate = constrain(target_rate, -10000, 10000);
|
target_rate = constrain(target_rate, -10000, 10000);
|
||||||
return get_rate_roll(target_rate);
|
return get_rate_roll(target_rate);
|
||||||
}
|
}
|
||||||
|
@ -91,7 +91,7 @@ get_acro_roll(int32_t target_rate)
|
||||||
static int
|
static int
|
||||||
get_acro_pitch(int32_t target_rate)
|
get_acro_pitch(int32_t target_rate)
|
||||||
{
|
{
|
||||||
target_rate = target_rate * g.pi_stabilize_pitch.kP();
|
target_rate = target_rate * g.acro_P;
|
||||||
target_rate = constrain(target_rate, -10000, 10000);
|
target_rate = constrain(target_rate, -10000, 10000);
|
||||||
return get_rate_pitch(target_rate);
|
return get_rate_pitch(target_rate);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue