diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index b623f3b644..48b09632cf 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal; /// global array with pointers to all APM RC channels, will be used by AP_Mount /// and AP_Camera classes / It points to RC input channels. -RC_Channel *RC_Channel::rc_ch[RC_MAX_CHANNELS]; +RC_Channel *RC_Channel::_rc_ch[RC_MAX_CHANNELS]; const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Param: MIN @@ -42,7 +42,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Range: 800 2200 // @Increment: 1 // @User: Advanced - AP_GROUPINFO_FLAGS("MIN", 0, RC_Channel, radio_min, 1100, AP_PARAM_NO_SHIFT), + AP_GROUPINFO_FLAGS("MIN", 0, RC_Channel, _radio_min, 1100, AP_PARAM_NO_SHIFT), // @Param: TRIM // @DisplayName: RC trim PWM @@ -51,7 +51,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Range: 800 2200 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("TRIM", 1, RC_Channel, radio_trim, 1500), + AP_GROUPINFO("TRIM", 1, RC_Channel, _radio_trim, 1500), // @Param: MAX // @DisplayName: RC max PWM @@ -60,7 +60,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Range: 800 2200 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("MAX", 2, RC_Channel, radio_max, 1900), + AP_GROUPINFO("MAX", 2, RC_Channel, _radio_max, 1900), // @Param: REV // @DisplayName: RC reverse @@ -175,20 +175,20 @@ RC_Channel::set_type_out(uint8_t t) void RC_Channel::trim() { - radio_trim = radio_in; + _radio_trim = _radio_in; } // read input from APM_RC - create a control_in value void RC_Channel::set_pwm(int16_t pwm) { - radio_in = pwm; + _radio_in = pwm; if (_type_in == RC_CHANNEL_TYPE_RANGE) { - control_in = pwm_to_range(); + _control_in = pwm_to_range(); } else { //RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW - control_in = pwm_to_angle(); + _control_in = pwm_to_angle(); } } @@ -199,8 +199,8 @@ void RC_Channel::set_pwm_all(void) { for (uint8_t i=0; iset_pwm(rc_ch[i]->read()); + if (_rc_ch[i] != NULL) { + _rc_ch[i]->set_pwm(_rc_ch[i]->read()); } } } @@ -211,13 +211,13 @@ RC_Channel::set_pwm_all(void) void RC_Channel::set_pwm_no_deadzone(int16_t pwm) { - radio_in = pwm; + _radio_in = pwm; if (_type_in == RC_CHANNEL_TYPE_RANGE) { - control_in = pwm_to_range_dz(0); + _control_in = pwm_to_range_dz(0); } else { //RC_CHANNEL_ANGLE, RC_CHANNEL_ANGLE_RAW - control_in = pwm_to_angle_dz(0); + _control_in = pwm_to_angle_dz(0); } } @@ -226,20 +226,20 @@ void RC_Channel::calc_pwm(void) { if(_type_out == RC_CHANNEL_TYPE_RANGE) { - pwm_out = range_to_pwm(); - radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out); + _pwm_out = range_to_pwm(); + _radio_out = (_reverse >= 0) ? (_radio_min + _pwm_out) : (_radio_max - _pwm_out); }else if(_type_out == RC_CHANNEL_TYPE_ANGLE_RAW) { - pwm_out = (float)servo_out * 0.1f; + _pwm_out = (float)_servo_out * 0.1f; int16_t reverse_mul = (_reverse==-1?-1:1); - radio_out = (pwm_out * reverse_mul) + radio_trim; + _radio_out = (_pwm_out * reverse_mul) + _radio_trim; }else{ // RC_CHANNEL_TYPE_ANGLE - pwm_out = angle_to_pwm(); - radio_out = pwm_out + radio_trim; + _pwm_out = angle_to_pwm(); + _radio_out = _pwm_out + _radio_trim; } - radio_out = constrain_int16(radio_out, radio_min.get(), radio_max.get()); + _radio_out = constrain_int16(_radio_out, _radio_min.get(), _radio_max.get()); } @@ -250,15 +250,15 @@ RC_Channel::calc_pwm(void) int16_t RC_Channel::get_control_mid() const { if (_type_in == RC_CHANNEL_TYPE_RANGE) { - int16_t r_in = (radio_min.get()+radio_max.get())/2; + int16_t r_in = (_radio_min.get()+_radio_max.get())/2; if (_reverse == -1) { - r_in = radio_max.get() - (r_in - radio_min.get()); + r_in = _radio_max.get() - (r_in - _radio_min.get()); } - int16_t radio_trim_low = radio_min + _dead_zone; + int16_t radio_trim_low = _radio_min + _dead_zone; - return (_low_in + ((int32_t)(_high_in - _low_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low)); + return (_low_in + ((int32_t)(_high_in - _low_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(_radio_max - radio_trim_low)); } else { return 0; } @@ -269,9 +269,9 @@ RC_Channel::get_control_mid() const { void RC_Channel::load_eeprom(void) { - radio_min.load(); - radio_trim.load(); - radio_max.load(); + _radio_min.load(); + _radio_trim.load(); + _radio_max.load(); _reverse.load(); _dead_zone.load(); } @@ -279,9 +279,9 @@ RC_Channel::load_eeprom(void) void RC_Channel::save_eeprom(void) { - radio_min.save(); - radio_trim.save(); - radio_max.save(); + _radio_min.save(); + _radio_trim.save(); + _radio_max.save(); _reverse.save(); _dead_zone.save(); } @@ -291,14 +291,14 @@ RC_Channel::save_eeprom(void) void RC_Channel::zero_min_max() { - radio_min = radio_max = radio_in; + _radio_min = _radio_max = _radio_in; } void RC_Channel::update_min_max() { - radio_min = MIN(radio_min.get(), radio_in); - radio_max = MAX(radio_max.get(), radio_in); + _radio_min = MIN(_radio_min.get(), _radio_in); + _radio_max = MAX(_radio_max.get(), _radio_in); } /* @@ -312,14 +312,14 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim) int16_t radio_trim_low = _trim - dead_zone; // prevent div by 0 - if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0) + 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_mul * ((int32_t)_high_in * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high); - }else if(radio_in < radio_trim_low) { - return reverse_mul * ((int32_t)_high_in * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min); + if(_radio_in > radio_trim_high) { + return reverse_mul * ((int32_t)_high_in * (int32_t)(_radio_in - radio_trim_high)) / (int32_t)(_radio_max - radio_trim_high); + }else if(_radio_in < radio_trim_low) { + return reverse_mul * ((int32_t)_high_in * (int32_t)(_radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - _radio_min); }else return 0; } @@ -331,7 +331,7 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim) int16_t RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) { - return pwm_to_angle_dz_trim(dead_zone, radio_trim); + return pwm_to_angle_dz_trim(dead_zone, _radio_trim); } /* @@ -349,10 +349,10 @@ int16_t RC_Channel::angle_to_pwm() { 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_out; + if((_servo_out * reverse_mul) > 0) { + return reverse_mul * ((int32_t)_servo_out * (int32_t)(_radio_max - _radio_trim)) / (int32_t)_high_out; } else { - return reverse_mul * ((int32_t)servo_out * (int32_t)(radio_trim - radio_min)) / (int32_t)_high_out; + return reverse_mul * ((int32_t)_servo_out * (int32_t)(_radio_trim - _radio_min)) / (int32_t)_high_out; } } @@ -363,16 +363,16 @@ RC_Channel::angle_to_pwm() int16_t RC_Channel::pwm_to_range_dz(uint16_t dead_zone) { - int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get()); + int16_t r_in = constrain_int16(_radio_in, _radio_min.get(), _radio_max.get()); if (_reverse == -1) { - r_in = radio_max.get() - (r_in - radio_min.get()); + r_in = _radio_max.get() - (r_in - _radio_min.get()); } - int16_t radio_trim_low = radio_min + dead_zone; + int16_t radio_trim_low = _radio_min + dead_zone; if (r_in > radio_trim_low) - return (_low_in + ((int32_t)(_high_in - _low_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low)); + return (_low_in + ((int32_t)(_high_in - _low_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(_radio_max - radio_trim_low)); else if (dead_zone > 0) return 0; else @@ -394,9 +394,9 @@ int16_t RC_Channel::range_to_pwm() { if (_high_out == _low_out) { - return radio_trim; + return _radio_trim; } - return ((int32_t)(servo_out - _low_out) * (int32_t)(radio_max - radio_min)) / (int32_t)(_high_out - _low_out); + return ((int32_t)(_servo_out - _low_out) * (int32_t)(_radio_max - _radio_min)) / (int32_t)(_high_out - _low_out); } // ------------------------------------------ @@ -406,16 +406,16 @@ RC_Channel::norm_input() { float ret; int16_t reverse_mul = (_reverse==-1?-1:1); - if (radio_in < radio_trim) { - if (radio_min >= radio_trim) { + if (_radio_in < _radio_trim) { + if (_radio_min >= _radio_trim) { return 0.0f; } - ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min); + ret = reverse_mul * (float)(_radio_in - _radio_trim) / (float)(_radio_trim - _radio_min); } else { - if (radio_max <= radio_trim) { + if (_radio_max <= _radio_trim) { return 0.0f; } - ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); + ret = reverse_mul * (float)(_radio_in - _radio_trim) / (float)(_radio_max - _radio_trim); } return constrain_float(ret, -1.0f, 1.0f); } @@ -423,14 +423,14 @@ RC_Channel::norm_input() float RC_Channel::norm_input_dz() { - int16_t dz_min = radio_trim - _dead_zone; - int16_t dz_max = radio_trim + _dead_zone; + 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_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min); - } else if (radio_in > dz_max && radio_max > dz_max) { - ret = reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max); + if (_radio_in < dz_min && 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_mul * (float)(_radio_in - dz_max) / (float)(_radio_max - dz_max); } else { ret = 0; } @@ -443,13 +443,13 @@ RC_Channel::norm_input_dz() uint8_t RC_Channel::percent_input() { - if (radio_in <= radio_min) { + if (_radio_in <= _radio_min) { return _reverse==-1?100:0; } - if (radio_in >= radio_max) { + if (_radio_in >= _radio_max) { return _reverse==-1?0:100; } - uint8_t ret = 100.0f * (radio_in - radio_min) / (float)(radio_max - radio_min); + uint8_t ret = 100.0f * (_radio_in - _radio_min) / (float)(_radio_max - _radio_min); if (_reverse == -1) { ret = 100 - ret; } @@ -459,15 +459,15 @@ RC_Channel::percent_input() float RC_Channel::norm_output() { - int16_t mid = (radio_max + radio_min) / 2; + int16_t mid = (_radio_max + _radio_min) / 2; float ret; - if (mid <= radio_min) { + if (mid <= _radio_min) { return 0; } - if (radio_out < mid) { - ret = (float)(radio_out - mid) / (float)(mid - radio_min); - } else if (radio_out > mid) { - ret = (float)(radio_out - mid) / (float)(radio_max - mid); + if (_radio_out < mid) { + ret = (float)(_radio_out - mid) / (float)(mid - _radio_min); + } else if (_radio_out > mid) { + ret = (float)(_radio_out - mid) / (float)(_radio_max - mid); } else { ret = 0; } @@ -479,19 +479,19 @@ RC_Channel::norm_output() void RC_Channel::output() const { - hal.rcout->write(_ch_out, radio_out); + hal.rcout->write(_ch_out, _radio_out); } void RC_Channel::output_trim() const { - hal.rcout->write(_ch_out, radio_trim); + hal.rcout->write(_ch_out, _radio_trim); } void RC_Channel::output_trim_all() { for (uint8_t i=0; ioutput_trim(); + if (_rc_ch[i] != NULL) { + _rc_ch[i]->output_trim(); } } } @@ -502,8 +502,8 @@ void RC_Channel::output_trim_all() void RC_Channel::setup_failsafe_trim_all() { for (uint8_t i=0; iset_failsafe_pwm(1U<radio_trim); + if (_rc_ch[i] != NULL) { + hal.rcout->set_failsafe_pwm(1U<_radio_trim); } } } @@ -511,7 +511,7 @@ void RC_Channel::setup_failsafe_trim_all() void RC_Channel::input() { - radio_in = hal.rcin->read(_ch_out); + _radio_in = hal.rcin->read(_ch_out); } uint16_t @@ -537,7 +537,7 @@ RC_Channel *RC_Channel::rc_channel(uint8_t i) if (i >= RC_MAX_CHANNELS) { return NULL; } - return rc_ch[i]; + return _rc_ch[i]; } // return a limit PWM value @@ -545,14 +545,14 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const { switch (limit) { case RC_CHANNEL_LIMIT_TRIM: - return radio_trim; + return _radio_trim; case RC_CHANNEL_LIMIT_MAX: - return get_reverse() ? radio_min : radio_max; + return get_reverse() ? _radio_min : _radio_max; case RC_CHANNEL_LIMIT_MIN: - return get_reverse() ? radio_max : radio_min; + return get_reverse() ? _radio_max : _radio_min; } // invalid limit value, return trim - return radio_trim; + return _radio_trim; } /* @@ -560,5 +560,5 @@ uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const */ bool RC_Channel::in_trim_dz() { - return is_bounded_int32(radio_in, radio_trim - _dead_zone, radio_trim + _dead_zone); + return is_bounded_int32(_radio_in, _radio_trim - _dead_zone, _radio_trim + _dead_zone); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 217cce0db7..75f59761bd 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -24,10 +24,11 @@ public: /// RC_Channel(uint8_t ch_out) : _high_in(1), - _ch_out(ch_out) { - AP_Param::setup_object_defaults(this, var_info); + _ch_out(ch_out) + { + AP_Param::setup_object_defaults(this, var_info); if (ch_out < RC_MAX_CHANNELS) { - rc_ch[ch_out] = this; + _rc_ch[ch_out] = this; } } @@ -76,88 +77,112 @@ public: // return a limit PWM value uint16_t get_limit_pwm(LimitValue limit) const; - // pwm is stored here - int16_t radio_in; - // call after first set_pwm void trim(); - // value generated from PWM - int16_t control_in; - - // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 - int16_t servo_out; - // generate PWM from servo_out value void calc_pwm(void); - // PWM is without the offset from radio_min - int16_t pwm_out; - int16_t radio_out; - - AP_Int16 radio_min; - AP_Int16 radio_trim; - AP_Int16 radio_max; - - // includes offset from PWM - //int16_t get_radio_out(void); - - int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim); - int16_t pwm_to_angle_dz(uint16_t dead_zone); - int16_t pwm_to_angle(); + int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim); + int16_t pwm_to_angle_dz(uint16_t dead_zone); + int16_t pwm_to_angle(); /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Ignore deadzone. */ - float norm_input(); + float norm_input(); /* return a normalised input for a channel, in range -1 to 1, centered around the channel trim. Take into account the deadzone */ - float norm_input_dz(); - - uint8_t percent_input(); - float norm_output(); - int16_t angle_to_pwm(); - int16_t pwm_to_range(); - int16_t pwm_to_range_dz(uint16_t dead_zone); - int16_t range_to_pwm(); - - void output() const; - void output_trim() const; - static void output_trim_all(); - static void setup_failsafe_trim_all(); - uint16_t read() const; - void input(); - void enable_out(); - void disable_out(); + float norm_input_dz(); + + uint8_t percent_input(); + float norm_output(); + int16_t angle_to_pwm(); + int16_t pwm_to_range(); + int16_t pwm_to_range_dz(uint16_t dead_zone); + int16_t range_to_pwm(); + void output() const; + void output_trim() const; + static void output_trim_all(); + static void setup_failsafe_trim_all(); + uint16_t read() const; + void input(); + void enable_out(); + void disable_out(); static const struct AP_Param::GroupInfo var_info[]; static RC_Channel *rc_channel(uint8_t i); - static RC_Channel **rc_channel_array(void) { - return rc_ch; + static RC_Channel **rc_channel_array(void) + { + return _rc_ch; } - bool in_trim_dz(); + bool in_trim_dz(); + + int16_t get_radio_in() const { return _radio_in;} + void set_radio_in(int16_t val){_radio_in = val;} + + int16_t get_control_in() const { return _control_in;} + void set_control_in(int16_t val) { _control_in = val;} + + int16_t get_servo_out() const {return _servo_out;} + void set_servo_out(int16_t val){_servo_out = val;} + + int16_t get_pwm_out() const { return _pwm_out;} + int16_t get_radio_out() const { return _radio_out;} + void set_radio_out(int16_t val){ _radio_out = val;} + + int16_t get_radio_min() const {return _radio_min.get();} + void set_radio_min(int16_t val){_radio_min = val;} + + int16_t get_radio_max() const {return _radio_max.get();} + void set_radio_max(int16_t val){_radio_max = val;} + + int16_t get_radio_trim() const { return _radio_trim.get();} + void set_radio_trim(int16_t val) { _radio_trim.set(val);} + void save_radio_trim() { _radio_trim.save();} + + bool min_max_configured() + { + return _radio_min.configured() && _radio_max.configured(); + } + private: - AP_Int8 _reverse; - AP_Int16 _dead_zone; - uint8_t _type_in; - int16_t _high_in; - int16_t _low_in; - uint8_t _type_out; - int16_t _high_out; - int16_t _low_out; - static RC_Channel *rc_ch[RC_MAX_CHANNELS]; + // pwm is stored here + int16_t _radio_in; + // value generated from PWM + int16_t _control_in; + // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 + int16_t _servo_out; + // PWM is without the offset from radio_min + int16_t _pwm_out; + int16_t _radio_out; + + AP_Int16 _radio_min; + AP_Int16 _radio_trim; + AP_Int16 _radio_max; + + AP_Int8 _reverse; + AP_Int16 _dead_zone; + uint8_t _type_in; + int16_t _high_in; + int16_t _low_in; + uint8_t _type_out; + int16_t _high_out; + int16_t _low_out; + + static RC_Channel *_rc_ch[RC_MAX_CHANNELS]; protected: - uint8_t _ch_out; + uint8_t _ch_out; }; // This is ugly, but it fixes poorly architected library