RC_Channel: added percent_input() function

useful for plane flaps
This commit is contained in:
Andrew Tridgell 2014-03-05 09:16:12 +11:00
parent fc85228d09
commit a788405c8b
2 changed files with 20 additions and 0 deletions

View File

@ -339,6 +339,25 @@ RC_Channel::norm_input()
return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
}
/*
get percentage input from 0 to 100. This ignores the trim value.
*/
uint8_t
RC_Channel::percent_input()
{
if (radio_in <= radio_min) {
return _reverse==-1?100:0;
}
if (radio_in >= radio_max) {
return _reverse==-1?0:100;
}
uint8_t ret = 100.0f * (radio_in - radio_min) / (float)(radio_max - radio_min);
if (_reverse == -1) {
ret = 100 - ret;
}
return ret;
}
float
RC_Channel::norm_output()
{

View File

@ -89,6 +89,7 @@ public:
int16_t pwm_to_angle_dz(uint16_t dead_zone);
int16_t pwm_to_angle();
float norm_input();
uint8_t percent_input();
float norm_output();
int16_t angle_to_pwm();
int16_t pwm_to_range();