Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Dead Zone fix - Now the range is full instead of clipped. Should make the Yaw much more smooth.

Browse Source
This commit is contained in:
Jason Short 2011-09-26 22:12:39 -07:00
parent 30f4ced9df
commit 1243e76f29
4 changed files with 62 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/radio.pde
View File

@ -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);
*/
}
}

13
ArduPlane/radio.pde
View File

@ -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);

58
libraries/RC_Channel/RC_Channel.cpp
View File

@ -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);
}

5
libraries/RC_Channel/RC_Channel.h
View File

@ -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;