diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0d9e2d4c68..0d466b480d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1080,20 +1080,20 @@ void update_throttle_mode(void) // calculate angle boost angle_boost = get_angle_boost(g.throttle_cruise); - + // manual command up or down? if(manual_boost != 0){ - + //remove alt_hold_velocity when implemented #if FRAME_CONFIG == HELI_FRAME g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + manual_boost + get_z_damping()); #else g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping(); #endif - + // reset next_WP.alt next_WP.alt = max(current_loc.alt, 100); - + }else{ // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ @@ -1102,13 +1102,12 @@ void update_throttle_mode(void) altitude_error = get_altitude_error(); // get the AP throttle - nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s - //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); + nav_throttle = get_nav_throttle(altitude_error); // clear the new data flag invalid_throttle = false; } - + #if FRAME_CONFIG == HELI_FRAME g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); #else @@ -1443,7 +1442,7 @@ static void tuning(){ g.heli_ext_gyro_gain = tuning_value * 1000; break; #endif - + case CH6_THR_HOLD_KP: g.rc_6.set_range(0,1000); // 0 to 1 g.pi_alt_hold.kP(tuning_value); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e1f0b07ffb..46e686c6f8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -71,7 +71,7 @@ // #ifndef CH7_OPTION -# define CH7_OPTION CH7_SET_HOVER +# define CH7_OPTION CH7_SAVE_WP #endif