mirror of https://github.com/ArduPilot/ardupilot
RC_Channel fix for throttle output.
throttle was outputting incorrectly and allowing the user to max out the throttle leaving nothing for attitude control
This commit is contained in:
parent
0adb5cb396
commit
c2d14a5cad
|
@ -66,6 +66,11 @@ static void init_rc_out()
|
||||||
read_radio();
|
read_radio();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// we want the input to be scaled correctly
|
||||||
|
g.rc_3.set_range_out(0,1000);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// sanity check - prevent unconfigured radios from outputting
|
// sanity check - prevent unconfigured radios from outputting
|
||||||
if(g.rc_3.radio_min >= 1300){
|
if(g.rc_3.radio_min >= 1300){
|
||||||
g.rc_3.radio_min = g.rc_3.radio_in;
|
g.rc_3.radio_min = g.rc_3.radio_in;
|
||||||
|
|
|
@ -37,9 +37,18 @@ const AP_Param::GroupInfo RC_Channel::var_info[] PROGMEM = {
|
||||||
void
|
void
|
||||||
RC_Channel::set_range(int low, int high)
|
RC_Channel::set_range(int low, int high)
|
||||||
{
|
{
|
||||||
_type = RC_CHANNEL_RANGE;
|
_type = RC_CHANNEL_RANGE;
|
||||||
_high = high;
|
_high = high;
|
||||||
_low = low;
|
_low = low;
|
||||||
|
_high_out = high;
|
||||||
|
_low_out = low;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
RC_Channel::set_range_out(int low, int high)
|
||||||
|
{
|
||||||
|
_high_out = high;
|
||||||
|
_low_out = low;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -79,8 +88,6 @@ void
|
||||||
RC_Channel::set_type(uint8_t t)
|
RC_Channel::set_type(uint8_t t)
|
||||||
{
|
{
|
||||||
_type = t;
|
_type = t;
|
||||||
//Serial.print("type1: ");
|
|
||||||
//Serial.println(t,DEC);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// call after first read
|
// call after first read
|
||||||
|
@ -92,9 +99,8 @@ RC_Channel::trim()
|
||||||
|
|
||||||
// read input from APM_RC - create a control_in value
|
// read input from APM_RC - create a control_in value
|
||||||
void
|
void
|
||||||
RC_Channel::set_pwm(int pwm)
|
RC_Channel::set_pwm(int16_t pwm)
|
||||||
{
|
{
|
||||||
//Serial.print(pwm,DEC);
|
|
||||||
|
|
||||||
/*if(_filter){
|
/*if(_filter){
|
||||||
if(radio_in == 0)
|
if(radio_in == 0)
|
||||||
|
@ -105,13 +111,12 @@ RC_Channel::set_pwm(int pwm)
|
||||||
radio_in = pwm;
|
radio_in = pwm;
|
||||||
}*/
|
}*/
|
||||||
|
|
||||||
radio_in = constrain(pwm, radio_min.get(), radio_max.get());
|
radio_in = pwm;
|
||||||
|
|
||||||
if(_type == RC_CHANNEL_RANGE){
|
if(_type == RC_CHANNEL_RANGE){
|
||||||
//Serial.print("range ");
|
|
||||||
control_in = pwm_to_range();
|
control_in = pwm_to_range();
|
||||||
//control_in = constrain(control_in, _low, _high);
|
//control_in = constrain(control_in, _low, _high);
|
||||||
control_in = min(control_in, _high);
|
//control_in = min(control_in, _high);
|
||||||
control_in = (control_in < _dead_zone) ? 0 : control_in;
|
control_in = (control_in < _dead_zone) ? 0 : control_in;
|
||||||
|
|
||||||
if (fabs(scale_output) != 1){
|
if (fabs(scale_output) != 1){
|
||||||
|
@ -243,10 +248,12 @@ RC_Channel::angle_to_pwm()
|
||||||
int16_t
|
int16_t
|
||||||
RC_Channel::pwm_to_range()
|
RC_Channel::pwm_to_range()
|
||||||
{
|
{
|
||||||
|
int16_t r_in = constrain(radio_in, radio_min.get(), radio_max.get());
|
||||||
|
|
||||||
int radio_trim_low = radio_min + _dead_zone;
|
int radio_trim_low = radio_min + _dead_zone;
|
||||||
|
|
||||||
if(radio_in > radio_trim_low)
|
if(r_in > radio_trim_low)
|
||||||
return (_low + ((long)(_high - _low) * (long)(radio_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
|
return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
|
||||||
else if(_dead_zone > 0)
|
else if(_dead_zone > 0)
|
||||||
return 0;
|
return 0;
|
||||||
else
|
else
|
||||||
|
@ -257,7 +264,7 @@ RC_Channel::pwm_to_range()
|
||||||
int16_t
|
int16_t
|
||||||
RC_Channel::range_to_pwm()
|
RC_Channel::range_to_pwm()
|
||||||
{
|
{
|
||||||
return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low);
|
return ((long)(servo_out - _low_out) * (long)(radio_max - radio_min)) / (long)(_high_out - _low_out);
|
||||||
}
|
}
|
||||||
|
|
||||||
// ------------------------------------------
|
// ------------------------------------------
|
||||||
|
|
|
@ -41,6 +41,7 @@ class RC_Channel{
|
||||||
|
|
||||||
// setup the control preferences
|
// setup the control preferences
|
||||||
void set_range(int low, int high);
|
void set_range(int low, int high);
|
||||||
|
void set_range_out(int low, int high);
|
||||||
void set_angle(int angle);
|
void set_angle(int angle);
|
||||||
void set_reverse(bool reverse);
|
void set_reverse(bool reverse);
|
||||||
bool get_reverse(void);
|
bool get_reverse(void);
|
||||||
|
@ -100,6 +101,8 @@ class RC_Channel{
|
||||||
uint8_t _type;
|
uint8_t _type;
|
||||||
int16_t _high;
|
int16_t _high;
|
||||||
int16_t _low;
|
int16_t _low;
|
||||||
|
int16_t _high_out;
|
||||||
|
int16_t _low_out;
|
||||||
};
|
};
|
||||||
|
|
||||||
// This is ugly, but it fixes compilation on arduino
|
// This is ugly, but it fixes compilation on arduino
|
||||||
|
|
Loading…
Reference in New Issue