Fixed issue with Simple mode changes.

Reverted to simpler, safer timer code for throttle hold
This commit is contained in:
Jason Short 2011-09-25 14:16:35 -07:00
parent 9de6117e4c
commit 059551de48
4 changed files with 3 additions and 17 deletions

View File

@ -1022,7 +1022,6 @@ void update_roll_pitch_mode(void)
int control_roll = 0, control_pitch = 0;
//read_radio();
if(do_simple && new_radio_frame){
new_radio_frame = false;
simple_timer++;
@ -1047,7 +1046,6 @@ void update_roll_pitch_mode(void)
g.rc_2.control_in = control_pitch;
}
switch(roll_pitch_mode){
case ROLL_PITCH_ACRO:
// Roll control

View File

@ -99,19 +99,7 @@ get_nav_throttle(long z_error, int target_speed)
rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -120, 140);
float delta_throttle;
// is the throttle_timer uninitialized?
if(throttle_timer == 0){
throttle_timer = millis();
delta_throttle = 0;
}else{
long timer = millis();
delta_throttle = (float)(timer - throttle_timer)/1000.0;
throttle_timer = timer;
}
return (int)g.pi_throttle.get_pi(rate_error, delta_throttle);
return (int)g.pi_throttle.get_pi(rate_error, .1);
}
static int

View File

@ -63,7 +63,7 @@
//
#ifndef CH7_OPTION
# define CH7_OPTION DO_SET_HOVER
# define CH7_OPTION CH7_SET_HOVER
#endif

View File

@ -14,7 +14,7 @@ static void read_control_switch()
set_mode(flight_modes[switchPosition]);
#if CH7_OPTION != SIMPLE_MODE_CONTROL
#if CH7_OPTION != CH7_SIMPLE_MODE
// setup Simple mode
// do we enable simple mode?
do_simple = (g.simple_modes & (1 << switchPosition));