mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Small fix to angle boost - increases by about 20% to deal with inefficiencies of non-downward thrust
This commit is contained in:
parent
d7fab41234
commit
0ccf5724aa
@ -434,15 +434,9 @@ get_nav_yaw_offset(int yaw_input, int reset)
|
||||
|
||||
static int16_t get_angle_boost(int16_t value)
|
||||
{
|
||||
// float temp = cos_pitch_x * cos_roll_x;
|
||||
// temp = 1.0 - constrain(temp, .5, 1.0);
|
||||
// int16_t output = temp * value;
|
||||
// return constrain(output, 0, 200);
|
||||
// return (int)(temp * value);
|
||||
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = constrain(temp, .5, 1.0);
|
||||
return ((float)g.throttle_cruise / temp) - g.throttle_cruise;
|
||||
return ((float)(g.throttle_cruise + 80) / temp) - (g.throttle_cruise + 80);
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
Loading…
Reference in New Issue
Block a user