mirror of https://github.com/ArduPilot/ardupilot
ACM: TradHeli
Removing Angle Boost function. Does not make sense for helis due to aerodynamics. Can cause more problems than it solves.
This commit is contained in:
parent
6056d4fb7b
commit
311d190494
|
@ -1675,7 +1675,7 @@ void update_throttle_mode(void)
|
|||
case THROTTLE_MANUAL:
|
||||
if (g.rc_3.control_in > 0) {
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in);
|
||||
g.rc_3.servo_out = g.rc_3.control_in;
|
||||
#else
|
||||
if (control_mode == ACRO) {
|
||||
g.rc_3.servo_out = g.rc_3.control_in;
|
||||
|
@ -1769,7 +1769,7 @@ void update_throttle_mode(void)
|
|||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle - landing_boost);
|
||||
throttle_out = g.throttle_cruise + nav_throttle - landing_boost;
|
||||
#else
|
||||
throttle_out = g.throttle_cruise + nav_throttle + angle_boost - landing_boost;
|
||||
#endif
|
||||
|
|
|
@ -585,20 +585,6 @@ static int16_t get_angle_boost(int16_t value)
|
|||
return ((float)(value + 80) / temp) - (value + 80);
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// heli_angle_boost - adds a boost depending on roll/pitch values
|
||||
// equivalent of quad's angle_boost function
|
||||
// throttle value should be 0 ~ 1000
|
||||
static int16_t heli_get_angle_boost(int16_t throttle)
|
||||
{
|
||||
float angle_boost_factor = cos_pitch_x * cos_roll_x;
|
||||
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
|
||||
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
|
||||
return throttle + throttle_above_mid*angle_boost_factor;
|
||||
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
// calculate modified roll/pitch depending upon optical flow calculated position
|
||||
static int32_t
|
||||
get_of_roll(int32_t input_roll)
|
||||
|
|
Loading…
Reference in New Issue