Browse Source

Copter: use scaled throttle for accel-throttle's I term

Accel throttle's I term is taken from scaled manual throttle
mission-4.1.18
Randy Mackay 12 years ago committed by rmackay9
parent
commit
a0b65a262b
  1. 2
      ArduCopter/ArduCopter.pde
  2. 4
      ArduCopter/Attitude.pde

2
ArduCopter/ArduCopter.pde

@ -1835,7 +1835,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) @@ -1835,7 +1835,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
set_accel_throttle_I_from_pilot_throttle();
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in));
}
throttle_initialised = true;
break;

4
ArduCopter/Attitude.pde

@ -1195,10 +1195,10 @@ static void reset_throttle_I(void) @@ -1195,10 +1195,10 @@ static void reset_throttle_I(void)
g.pid_throttle_accel.reset_I();
}
static void set_accel_throttle_I_from_pilot_throttle(void)
static void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_throttle_accel.set_integrator(g.rc_3.control_in-g.throttle_cruise);
g.pid_throttle_accel.set_integrator(pilot_throttle-g.throttle_cruise);
}
static void reset_stability_I(void)

Loading…
Cancel
Save