aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RC_Channel: fixed usage of _reverse to be consistent

Browse Source 
users could set RCn_REV to 0 and get very confusing results
This commit is contained in:
Andrew Tridgell 2015-02-18 12:47:56 +11:00
parent 307b9e807f
commit 6959cdbf15
1 changed files with 20 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/RC_Channel/RC_Channel.cpp
View File

@ -214,7 +214,8 @@ RC_Channel::calc_pwm(void)
}else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) {
pwm_out = (float)servo_out * 0.1f;
radio_out = (pwm_out * _reverse) + radio_trim;
int16_t reverse_mul = (_reverse==-1?-1:1);
radio_out = (pwm_out * reverse_mul) + radio_trim;
}else{ // RC_CHANNEL_TYPE_ANGLE
pwm_out = angle_to_pwm();
@ -297,10 +298,11 @@ RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
return 0;
int16_t reverse_mul = (_reverse==-1?-1:1);
if(radio_in > radio_trim_high) {
return _reverse * ((int32_t)_high * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high);
return reverse_mul * ((int32_t)_high * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high);
}else if(radio_in < radio_trim_low) {
return _reverse * ((int32_t)_high * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min);
return reverse_mul * ((int32_t)_high * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min);
}else
return 0;
}
@ -319,10 +321,12 @@ RC_Channel::pwm_to_angle()
int16_t
RC_Channel::angle_to_pwm()
{
if((servo_out * _reverse) > 0)
return _reverse * ((int32_t)servo_out * (int32_t)(radio_max - radio_trim)) / (int32_t)_high;
else
return _reverse * ((int32_t)servo_out * (int32_t)(radio_trim - radio_min)) / (int32_t)_high;
int16_t reverse_mul = (_reverse==-1?-1:1);
if((servo_out * reverse_mul) > 0) {
return reverse_mul * ((int32_t)servo_out * (int32_t)(radio_max - radio_trim)) / (int32_t)_high;
} else {
return reverse_mul * ((int32_t)servo_out * (int32_t)(radio_trim - radio_min)) / (int32_t)_high;
}
}
/*
@ -374,10 +378,12 @@ float
RC_Channel::norm_input()
{
float ret;
if(radio_in < radio_trim)
ret = _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
else
ret = _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
int16_t reverse_mul = (_reverse==-1?-1:1);
if (radio_in < radio_trim) {
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
} else {
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
}
return constrain_float(ret, -1.0f, 1.0f);
}
@ -387,10 +393,11 @@ RC_Channel::norm_input_dz()
int16_t dz_min = radio_trim - _dead_zone;
int16_t dz_max = radio_trim + _dead_zone;
float ret;
int16_t reverse_mul = (_reverse==-1?-1:1);
if (radio_in < dz_min && dz_min > radio_min) {
ret = _reverse * (float)(radio_in - dz_min) / (float)(dz_min - radio_min);
ret = reverse_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min);
} else if (radio_in > dz_max && radio_max > dz_max) {
ret = _reverse * (float)(radio_in - dz_max) / (float)(radio_max - dz_max);
ret = reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max);
} else {
ret = 0;
}