From 1243e76f29e832b82a653eea6bd565ebc112c8d7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 26 Sep 2011 22:12:39 -0700 Subject: [PATCH] Dead Zone fix - Now the range is full instead of clipped. Should make the Yaw much more smooth. --- ArduCopter/radio.pde | 18 ++++----- ArduPlane/radio.pde | 17 ++++++--- libraries/RC_Channel/RC_Channel.cpp | 58 ++++++++++++++++++++--------- libraries/RC_Channel/RC_Channel.h | 5 ++- 4 files changed, 62 insertions(+), 36 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 86b30c9de7..0b092aa8bb 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -23,10 +23,16 @@ static void init_rc_in() g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); // set rc dead zones - g.rc_1.dead_zone = 60; // 60 = .6 degrees + /*g.rc_1.dead_zone = 60; g.rc_2.dead_zone = 60; g.rc_3.dead_zone = 60; - g.rc_4.dead_zone = 600;// 0 = hybrid rate approach + g.rc_4.dead_zone = 300; + */ + + g.rc_1.set_dead_zone(60); + g.rc_2.set_dead_zone(60); + g.rc_3.set_dead_zone(60); + g.rc_4.set_dead_zone(300); //set auxiliary ranges g.rc_5.set_range(0,1000); @@ -117,14 +123,6 @@ static void read_radio() #endif //throttle_failsafe(g.rc_3.radio_in); - - /* - Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), - g.rc_1.control_in, - g.rc_2.control_in, - g.rc_3.control_in, - g.rc_4.control_in); - */ } } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 6c8967d542..af2c38bd88 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -17,10 +17,15 @@ static void init_rc_in() g.channel_throttle.set_range(0, 100); // set rc dead zones - g.channel_roll.dead_zone = 60; - g.channel_pitch.dead_zone = 60; - g.channel_rudder.dead_zone = 60; - g.channel_throttle.dead_zone = 6; + g.channel_roll.set_dead_zone(60); + g.channel_pitch.set_dead_zone(60); + g.channel_rudder.set_dead_zone(60); + g.channel_throttle.set_dead_zone(6); + + //g.channel_roll.dead_zone = 60; + //g.channel_pitch.dead_zone = 60; + //g.channel_rudder.dead_zone = 60; + //g.channel_throttle.dead_zone = 6; //set auxiliary ranges update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); @@ -112,7 +117,7 @@ static void control_failsafe(uint16_t pwm) } else { ch3_failsafe = false; } - + //Check for failsafe and debounce funky reads } else if (g.throttle_fs_enabled) { if (pwm < (unsigned)g.throttle_fs_value){ @@ -155,7 +160,7 @@ static void trim_control_surfaces() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel - + }else{ elevon1_trim = ch1_temp; elevon2_trim = ch2_temp; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 3b6a6c545d..11af18e10a 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -35,6 +35,12 @@ RC_Channel::set_angle(int angle) _high = angle; } +void +RC_Channel::set_dead_zone(int dzone) +{ + _dead_zone = abs(dzone >>1); +} + void RC_Channel::set_reverse(bool reverse) { @@ -88,18 +94,25 @@ RC_Channel::set_pwm(int pwm) if(_type == RC_CHANNEL_RANGE){ //Serial.print("range "); control_in = pwm_to_range(); - control_in = (control_in < dead_zone) ? 0 : control_in; - //if (fabs(scale_output) > 0){ - // control_in *= scale_output; - //} - }else{ - control_in = pwm_to_angle(); - control_in = (abs(control_in) < dead_zone) ? 0 : control_in; + control_in = (control_in < _dead_zone) ? 0 : control_in; + + if (fabs(scale_output) != 1){ + control_in *= scale_output; + } + + }else{ + + //RC_CHANNEL_ANGLE, RC_CHANNEL_ANGLE_RAW + control_in = pwm_to_angle(); + // deadzone moved to + //control_in = (abs(control_in) < _dead_zone) ? 0 : control_in; + + if (fabs(scale_output) != 1){ + control_in *= scale_output; + } - //if (fabs(scale_output) > 0){ - // control_in *= scale_output; - //} /* + // coming soon ?? if(expo) { long temp = control_in; temp = (temp * temp) / (long)_high; @@ -133,7 +146,7 @@ RC_Channel::calc_pwm(void) pwm_out = (float)servo_out * .1; radio_out = (pwm_out * _reverse) + radio_trim; - }else{ + }else{ // RC_CHANNEL_ANGLE pwm_out = angle_to_pwm(); radio_out = pwm_out + radio_trim; } @@ -175,10 +188,15 @@ RC_Channel::update_min_max() int16_t RC_Channel::pwm_to_angle() { - if(radio_in > radio_trim) - return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max - radio_trim); - else - return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min); + int radio_trim_high = radio_trim + _dead_zone; + int radio_trim_low = radio_trim - _dead_zone; + + if(radio_in > radio_trim_high){ + return _reverse * ((long)_high * (long)(radio_in - radio_trim_high)) / (long)(radio_max - radio_trim_high); + }else if(radio_in < radio_trim_low){ + return _reverse * ((long)_high * (long)(radio_in - radio_trim_low)) / (long)(radio_trim_low - radio_min); + }else + return 0; } @@ -196,14 +214,18 @@ RC_Channel::angle_to_pwm() int16_t RC_Channel::pwm_to_range() { - //return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); - return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min)); + int radio_trim_low = radio_min + _dead_zone; + + if(radio_in > radio_trim_low) + return (_low + ((long)(_high - _low) * (long)(radio_in - radio_trim_low)) / (long)(radio_max - radio_trim_low)); + else + return 0; } + int16_t RC_Channel::range_to_pwm() { - //return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min)); return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 922b15540e..9ce43fd1c0 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -28,7 +28,7 @@ class RC_Channel{ _high(1), _filter(true), _reverse (&_group, 3, 1, name ? PSTR("REV") : 0), - dead_zone(0), + _dead_zone(0), scale_output(1.0) {} @@ -48,6 +48,7 @@ class RC_Channel{ void set_angle(int angle); void set_reverse(bool reverse); bool get_reverse(void); + void set_dead_zone(int dzone); // read input from APM_RC - create a control_in value void set_pwm(int pwm); @@ -63,7 +64,6 @@ class RC_Channel{ // value generated from PWM int16_t control_in; - int16_t dead_zone; // used to keep noise down and create a dead zone. int control_mix(float value); @@ -97,6 +97,7 @@ class RC_Channel{ bool _filter; AP_Int8 _reverse; + int16_t _dead_zone; // used to keep noise down and create a dead zone. uint8_t _type; int16_t _high; int16_t _low;