diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c5800cb1a3..0d3f343a91 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -29,9 +29,7 @@ extern const AP_HAL::HAL& hal; #include "RC_Channel.h" -/// 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_Channels::channels; const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Param: MIN @@ -41,7 +39,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("MIN", 1, RC_Channel, radio_min, 1100), // @Param: TRIM // @DisplayName: RC trim PWM @@ -50,7 +48,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", 2, RC_Channel, radio_trim, 1500), // @Param: MAX // @DisplayName: RC max PWM @@ -59,19 +57,14 @@ 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", 3, RC_Channel, radio_max, 1900), - // @Param: REV - // @DisplayName: RC reverse - // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel. - // @Values: -1:Reversed,1:Normal + // @Param: REVERSED + // @DisplayName: RC reversed + // @Description: Reverse servo operation. Set to 0 for normal (forward) operation. Set to 1 to reverse this input channel. + // @Values: 0:Normal,1:Reversed // @User: Advanced - AP_GROUPINFO("REV", 3, RC_Channel, _reverse, 1), - - // Note: index 4 was used by the previous _dead_zone value. We - // changed it to 5 as dead zone values had previously been - // incorrectly saved, overriding user values. They were also - // incorrectly interpreted for the throttle on APM:Plane + AP_GROUPINFO("REVERSED", 4, RC_Channel, reversed, 0), // @Param: DZ // @DisplayName: RC dead-zone @@ -79,115 +72,140 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Units: pwm // @Range: 0 200 // @User: Advanced - AP_GROUPINFO("DZ", 5, RC_Channel, _dead_zone, 0), + AP_GROUPINFO("DZ", 5, RC_Channel, dead_zone, 0), AP_GROUPEND }; -// setup the control preferences -void -RC_Channel::set_range(int16_t low, int16_t high) + +const AP_Param::GroupInfo RC_Channels::var_info[] = { + // @Group: 1_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[0], "1_", 1, RC_Channels, RC_Channel), + + // @Group: 2_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[1], "2_", 2, RC_Channels, RC_Channel), + + // @Group: 3_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[2], "3_", 3, RC_Channels, RC_Channel), + + // @Group: 4_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[3], "4_", 4, RC_Channels, RC_Channel), + + // @Group: 5_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[4], "5_", 5, RC_Channels, RC_Channel), + + // @Group: 6_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[5], "6_", 6, RC_Channels, RC_Channel), + + // @Group: 7_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[6], "7_", 7, RC_Channels, RC_Channel), + + // @Group: 8_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[7], "8_", 8, RC_Channels, RC_Channel), + + // @Group: 9_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[8], "9_", 9, RC_Channels, RC_Channel), + + // @Group: 10_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[9], "10_", 10, RC_Channels, RC_Channel), + + // @Group: 11_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[10], "11_", 11, RC_Channels, RC_Channel), + + // @Group: 12_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[11], "12_", 12, RC_Channels, RC_Channel), + + // @Group: 13_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[12], "13_", 13, RC_Channels, RC_Channel), + + // @Group: 14_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[13], "14_", 14, RC_Channels, RC_Channel), + + // @Group: 15_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[14], "15_", 15, RC_Channels, RC_Channel), + + // @Group: 16_ + // @Path: RC_Channel.cpp + AP_SUBGROUPINFO(obj_channels[15], "16_", 16, RC_Channels, RC_Channel), + + AP_GROUPEND +}; + + +// constructor +RC_Channel::RC_Channel(void) { - set_range_in(low, high); - set_range_out(low, high); + AP_Param::setup_object_defaults(this, var_info); +} + +/* + channels group object constructor + */ +RC_Channels::RC_Channels(void) +{ + channels = obj_channels; + + // set defaults from the parameter table + AP_Param::setup_object_defaults(this, var_info); + + // setup ch_in on channels + for (uint8_t i=0; iset_pwm(_rc_ch[i]->read()); - } + for (uint8_t i=0; i= 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; - 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(); - _radio_out = _pwm_out + _radio_trim; - } - - _radio_out = constrain_int16(_radio_out, _radio_min.get(), _radio_max.get()); -} - - /* return the center stick position expressed as a control_in value used for thr_mid in copter */ -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 RC_Channel::get_control_mid() const +{ + if (type_in == RC_CHANNEL_TYPE_RANGE) { + int16_t r_in = (radio_min.get() + radio_max.get())/2; - if (_reverse == -1) { - r_in = _radio_max.get() - (r_in - _radio_min.get()); + if (reversed) { + 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 (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low)); } else { return 0; } @@ -265,39 +259,22 @@ RC_Channel::get_control_mid() const { // ------------------------------------------ -void -RC_Channel::load_eeprom(void) +void RC_Channel::load_eeprom(void) { - _radio_min.load(); - _radio_trim.load(); - _radio_max.load(); - _reverse.load(); - _dead_zone.load(); + radio_min.load(); + radio_trim.load(); + radio_max.load(); + reversed.load(); + dead_zone.load(); } -void -RC_Channel::save_eeprom(void) +void RC_Channel::save_eeprom(void) { - _radio_min.save(); - _radio_trim.save(); - _radio_max.save(); - _reverse.save(); - _dead_zone.save(); -} - -// ------------------------------------------ - -void -RC_Channel::zero_min_max() -{ - _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.save(); + radio_trim.save(); + radio_max.save(); + reversed.save(); + dead_zone.save(); } /* @@ -305,22 +282,23 @@ RC_Channel::update_min_max() the current radio_in value using the specified dead_zone */ int16_t -RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim) +RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t trim) { - int16_t radio_trim_high = _trim + dead_zone; - int16_t radio_trim_low = _trim - dead_zone; + int16_t radio_trim_high = trim + _dead_zone; + 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); - }else + int16_t reverse_mul = (reversed?-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); + } else { return 0; + } } /* @@ -328,9 +306,9 @@ RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim) the current radio_in value using the specified dead_zone */ int16_t -RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) +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); } /* @@ -340,42 +318,29 @@ RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) int16_t RC_Channel::pwm_to_angle() { - return pwm_to_angle_dz(_dead_zone); + return pwm_to_angle_dz(dead_zone); } -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; - } else { - return reverse_mul * ((int32_t)_servo_out * (int32_t)(_radio_trim - _radio_min)) / (int32_t)_high_out; - } -} - /* convert a pulse width modulation value to a value in the configured range, using the specified deadzone */ int16_t -RC_Channel::pwm_to_range_dz(uint16_t dead_zone) +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()); + if (reversed) { + 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)); - else if (dead_zone > 0) - return 0; - else - return _low_in; + if (r_in > radio_trim_low) { + return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low)); + } + return 0; } /* @@ -385,17 +350,16 @@ RC_Channel::pwm_to_range_dz(uint16_t dead_zone) int16_t RC_Channel::pwm_to_range() { - return pwm_to_range_dz(_dead_zone); + return pwm_to_range_dz(dead_zone); } -int16_t -RC_Channel::range_to_pwm() +int16_t RC_Channel::get_control_in_zero_dz(void) { - if (_high_out == _low_out) { - return _radio_trim; + if (type_in == RC_CHANNEL_TYPE_RANGE) { + return pwm_to_range_dz(0); } - return ((int32_t)(_servo_out - _low_out) * (int32_t)(_radio_max - _radio_min)) / (int32_t)(_high_out - _low_out); + return pwm_to_angle_dz(0); } // ------------------------------------------ @@ -404,17 +368,17 @@ float RC_Channel::norm_input() { float ret; - int16_t reverse_mul = (_reverse==-1?-1:1); - if (_radio_in < _radio_trim) { - if (_radio_min >= _radio_trim) { + int16_t reverse_mul = (reversed?-1:1); + 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); } @@ -422,14 +386,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); + int16_t reverse_mul = (reversed?-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); } else { ret = 0; } @@ -442,125 +406,29 @@ RC_Channel::norm_input_dz() uint8_t RC_Channel::percent_input() { - if (_radio_in <= _radio_min) { - return _reverse==-1?100:0; + if (radio_in <= radio_min) { + return reversed?100:0; } - if (_radio_in >= _radio_max) { - return _reverse==-1?0:100; + if (radio_in >= radio_max) { + return reversed?0:100; } - uint8_t ret = 100.0f * (_radio_in - _radio_min) / (float)(_radio_max - _radio_min); - if (_reverse == -1) { + uint8_t ret = 100.0f * (radio_in - radio_min) / (float)(radio_max - radio_min); + if (reversed) { ret = 100 - ret; } return ret; } -float -RC_Channel::norm_output() -{ - int16_t mid = (_radio_max + _radio_min) / 2; - float ret; - 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); - } else { - ret = 0; - } - if (_reverse == -1) { - ret = -ret; - } - return ret; -} - -void RC_Channel::output() const -{ - hal.rcout->write(_ch_out, _radio_out); -} - -void RC_Channel::output_trim() -{ - _radio_out = _radio_trim; - output(); -} - -void RC_Channel::output_trim_all() -{ - for (uint8_t i=0; ioutput_trim(); - } - } -} - -/* - setup the failsafe value to the trim value for all channels in chmask - */ -void RC_Channel::setup_failsafe_trim_mask(uint16_t chmask) -{ - for (uint8_t i=0; iset_failsafe_pwm(1U<_radio_trim); - } - } -} - -/* - setup the failsafe value to the trim value for all channels - */ -void RC_Channel::setup_failsafe_trim_all() -{ - setup_failsafe_trim_mask(0xFFFF); -} - void RC_Channel::input() { - _radio_in = hal.rcin->read(_ch_out); + radio_in = hal.rcin->read(ch_in); } uint16_t RC_Channel::read() const { - return hal.rcin->read(_ch_out); -} - -void -RC_Channel::enable_out() -{ - hal.rcout->enable_ch(_ch_out); -} - -void -RC_Channel::disable_out() -{ - hal.rcout->disable_ch(_ch_out); -} - -RC_Channel *RC_Channel::rc_channel(uint8_t i) -{ - if (i >= RC_MAX_CHANNELS) { - return nullptr; - } - return _rc_ch[i]; -} - -// return a limit PWM value -uint16_t RC_Channel::get_limit_pwm(LimitValue limit) const -{ - switch (limit) { - case RC_CHANNEL_LIMIT_TRIM: - return _radio_trim; - case RC_CHANNEL_LIMIT_MAX: - return get_reverse() ? _radio_min : _radio_max; - case RC_CHANNEL_LIMIT_MIN: - return get_reverse() ? _radio_max : _radio_min; - } - // invalid limit value, return trim - return _radio_trim; + return hal.rcin->read(ch_in); } /* @@ -568,45 +436,6 @@ 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); } - -/* - return the current radio_out value normalised as a float with 1.0 - being full output and 0.0 being zero output, taking into account - output type and reversals - - For angle outputs the returned value is from -1 to 1 - - For range outputs the returned value is from 0 to 1 - */ -float RC_Channel::get_radio_out_normalised(uint16_t pwm) const -{ - if (_radio_max <= _radio_min) { - return 0; - } - float ret; - if (_type_out == RC_CHANNEL_TYPE_RANGE) { - if (pwm <= _radio_min) { - ret = 0; - } else if (pwm >= _radio_max) { - ret = 1; - } else { - ret = (pwm - _radio_min) / float(_radio_max - _radio_min); - } - if (_reverse == -1) { - ret = 1 - ret; - } - } else { - if (pwm < _radio_trim) { - ret = -(_radio_trim - pwm) / float(_radio_trim - _radio_min); - } else { - ret = (pwm - _radio_trim) / float(_radio_max - _radio_trim); - } - if (_reverse == -1) { - ret = -ret; - } - } - return ret; -} diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index dd214bf455..ec815395ec 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,28 +7,16 @@ #define RC_CHANNEL_TYPE_ANGLE 0 #define RC_CHANNEL_TYPE_RANGE 1 -#define RC_CHANNEL_TYPE_ANGLE_RAW 2 -#define RC_MAX_CHANNELS 14 +#define NUM_RC_CHANNELS 16 /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel { public: - /// Constructor - /// - /// @param key EEPROM storage key for the channel trim parameters. - /// @param name Optional name for the group. - /// - RC_Channel(uint8_t ch_out) : - _high_in(1), - _ch_out(ch_out) - { - AP_Param::setup_object_defaults(this, var_info); - if (ch_out < RC_MAX_CHANNELS) { - _rc_ch[ch_out] = this; - } - } + friend class RC_Channels; + // Constructor + RC_Channel(void); // used to get min/max/trim limit value based on _reverse enum LimitValue { @@ -37,52 +25,29 @@ public: RC_CHANNEL_LIMIT_MAX }; - // setup min and max radio values in CLI - void update_min_max(); - void zero_min_max(); - // startup void load_eeprom(void); void save_eeprom(void); void save_trim(void); - void set_type(uint8_t t); - void set_type_in(uint8_t t); - void set_type_out(uint8_t t); // setup the control preferences - void set_range(int16_t low, int16_t high); - void set_range_out(int16_t low, int16_t high); - void set_range_in(int16_t low, int16_t high); - void set_angle(int16_t angle); - void set_angle_in(int16_t angle); - void set_angle_out(int16_t angle); - void set_reverse(bool reverse); + void set_range(uint16_t high); + void set_angle(uint16_t angle); bool get_reverse(void) const; void set_default_dead_zone(int16_t dzone); - uint16_t get_dead_zone(void) const { return _dead_zone; } + uint16_t get_dead_zone(void) const { return dead_zone; } - // get the channel number - uint8_t get_ch_out(void) const { return _ch_out; } - // get the center stick position expressed as a control_in value int16_t get_control_mid() const; // read input from APM_RC - create a control_in value void set_pwm(int16_t pwm); - static void set_pwm_all(void); void set_pwm_no_deadzone(int16_t pwm); - // return a limit PWM value - uint16_t get_limit_pwm(LimitValue limit) const; - // call after first set_pwm void trim(); - // generate PWM from servo_out value - void calc_pwm(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(); /* @@ -98,98 +63,82 @@ public: 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(); - static void output_trim_all(); - static void setup_failsafe_trim_mask(uint16_t chmask); - 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 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; - } - 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_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_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();} - - // return output type RC_CHANNEL_TYPE_* - uint8_t get_type_out(void) const { return _type_out; } - - // get the current radio_out value as a floating point number - // normalised so that 1.0 is full output - float get_radio_out_normalised(uint16_t pwm) const; + // get control input with zero deadzone + int16_t get_control_in_zero_dz(void); + 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(); + return radio_min.configured() && radio_max.configured(); } private: // 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; + int16_t radio_in; - AP_Int16 _radio_min; - AP_Int16 _radio_trim; - AP_Int16 _radio_max; + // value generated from PWM normalised to configured scale + int16_t control_in; + + 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; + AP_Int8 reversed; + AP_Int16 dead_zone; - static RC_Channel *_rc_ch[RC_MAX_CHANNELS]; + uint8_t type_in; + int16_t high_in; -protected: - uint8_t _ch_out; + // the input channel this corresponds to + uint8_t ch_in; + + int16_t pwm_to_angle_dz(uint16_t dead_zone); }; -// This is ugly, but it fixes poorly architected library -#include "RC_Channel_aux.h" + +/* + class RC_Channels. Hold the full set of RC_Channel objects +*/ +class RC_Channels { +public: + // constructor + RC_Channels(void); + + static const struct AP_Param::GroupInfo var_info[]; + + static RC_Channel *rc_channel(uint8_t chan) { + return (chan < NUM_RC_CHANNELS)?&channels[chan]:nullptr; + } + + static void set_pwm_all(void); + +private: + // this static arrangement is to avoid static pointers in AP_Param tables + static RC_Channel *channels; + RC_Channel obj_channels[NUM_RC_CHANNELS]; +}; diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp deleted file mode 100644 index 2db7adaa1e..0000000000 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ /dev/null @@ -1,505 +0,0 @@ -#include "RC_Channel_aux.h" - -#include -#include -extern const AP_HAL::HAL& hal; - -const AP_Param::GroupInfo RC_Channel_aux::var_info[] = { - AP_NESTEDGROUPINFO(RC_Channel, 0), - - // @Param: FUNCTION - // @DisplayName: Servo out function - // @Description: Setting this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function - // @Values: 0:Disabled,1:RCPassThru,2:Flap,3:Flap_auto,4:Aileron,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release,12:mount2_pan,13:mount2_tilt,14:mount2_roll,15:mount2_open,16:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,68:Choke,69:Starter,70:Throttle - // @User: Standard - AP_GROUPINFO("FUNCTION", 1, RC_Channel_aux, function, 0), - - AP_GROUPEND -}; - -RC_Channel_aux *RC_Channel_aux::_aux_channels[RC_AUX_MAX_CHANNELS]; -uint64_t RC_Channel_aux::_function_mask[2]; -bool RC_Channel_aux::_initialised; -bool RC_Channel_aux::_disable_passthrough; - -void -RC_Channel_aux::set_function_mask(uint8_t fn) -{ - uint8_t idx = fn / 64; - uint8_t bit = fn % 64; - _function_mask[idx] |= (1ULL<<(uint8_t)bit); -} - -void -RC_Channel_aux::clear_function_mask(void) -{ - memset(_function_mask, 0, sizeof(_function_mask)); -} - -/// map a function to a servo channel and output it -void -RC_Channel_aux::output_ch(void) -{ - // take care of two corner cases - switch(function) - { - case k_none: // disabled - return; - case k_manual: // manual - if (_disable_passthrough) { - set_radio_out(get_radio_trim()); - } else { - set_radio_out(get_radio_in()); - } - break; - case k_rcin1 ... k_rcin16: // rc pass-thru - if (_disable_passthrough) { - set_radio_out(get_radio_trim()); - } else { - set_radio_out(hal.rcin->read(function-k_rcin1)); - } - break; - case k_motor1 ... k_motor8: - // handled by AP_Motors::rc_write() - return; - } - hal.rcout->write(_ch_out, get_radio_out()); -} - -/* - call output_ch() on all auxiliary channels - */ -void -RC_Channel_aux::output_ch_all(void) -{ - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i]) { - _aux_channels[i]->output_ch(); - } - } -} - -/* - prevent a channel from being used for auxiliary functions - This is used by the copter code to ensure channels used for motors - can't be used for auxiliary functions -*/ -void RC_Channel_aux::disable_aux_channel(uint8_t channel) -{ - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->_ch_out == channel) { - _aux_channels[i] = nullptr; - } - } -} - -/* - return the current function for a channel -*/ -RC_Channel_aux::Aux_servo_function_t RC_Channel_aux::channel_function(uint8_t channel) -{ - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->_ch_out == channel) { - return (RC_Channel_aux::Aux_servo_function_t)_aux_channels[i]->function.get(); - } - } - return RC_Channel_aux::k_none; -} - -/* - setup a channels aux servo function -*/ -void RC_Channel_aux::aux_servo_function_setup(void) -{ - switch (function) { - case RC_Channel_aux::k_flap: - case RC_Channel_aux::k_flap_auto: - case RC_Channel_aux::k_egg_drop: - set_range_out(0,100); - break; - case RC_Channel_aux::k_heli_rsc: - case RC_Channel_aux::k_heli_tail_rsc: - set_range_out(0,1000); - break; - case RC_Channel_aux::k_aileron_with_input: - case RC_Channel_aux::k_elevator_with_input: - set_angle(4500); - break; - case RC_Channel_aux::k_aileron: - case RC_Channel_aux::k_elevator: - case RC_Channel_aux::k_dspoiler1: - case RC_Channel_aux::k_dspoiler2: - case RC_Channel_aux::k_rudder: - case RC_Channel_aux::k_steering: - case RC_Channel_aux::k_flaperon1: - case RC_Channel_aux::k_flaperon2: - set_angle_out(4500); - break; - case RC_Channel_aux::k_motor_tilt: - // tenth percentage tilt - set_range_out(0,1000); - break; - case RC_Channel_aux::k_throttle: - // fixed wing throttle - set_range_out(0,100); - break; - default: - break; - } - - if (function < k_nr_aux_servo_functions) { - set_function_mask((uint8_t)function.get()); - } -} - -/// Update the _aux_channels array of pointers to rc_x channels -/// This is to be done before rc_init so that the channels get correctly initialized. -/// It also should be called periodically because the user might change the configuration and -/// expects the changes to take effect instantly -/// Supports up to eight aux servo outputs (typically CH5 ... CH11) -/// All servos must be configured with a single call to this function -/// (do not call this twice with different parameters, the second call will reset the effect of the first call) -void RC_Channel_aux::update_aux_servo_function(void) -{ - clear_function_mask(); - - // set auxiliary ranges - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] == nullptr) continue; - _aux_channels[i]->aux_servo_function_setup(); - } - _initialised = true; -} - - -/// Should be called after the the servo functions have been initialized -void RC_Channel_aux::enable_aux_servos() -{ - update_aux_servo_function(); - - // enable all channels that are not set to a valid function. This - // includes k_none servos, which allows those to get their initial - // trim value on startup - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i]) { - RC_Channel_aux::Aux_servo_function_t function = (RC_Channel_aux::Aux_servo_function_t)_aux_channels[i]->function.get(); - // see if it is a valid function - if (function < RC_Channel_aux::k_nr_aux_servo_functions) { - _aux_channels[i]->enable_out(); - } - } - } -} - -/* - set radio_out for all channels matching the given function type - */ -void -RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t value) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - _aux_channels[i]->set_radio_out(value); - _aux_channels[i]->output(); - } - } -} - -/* - get radio_out for *first* channel matching the given function type. - Returns true if a value was found. - */ -bool RC_Channel_aux::get_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t &value) -{ - if (!function_assigned(function)) { - return false; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - value = _aux_channels[i]->get_radio_out(); - return true; - } - } - return false; -} - -/* - set radio_out for all channels matching the given function type, allow radio_trim to center servo - */ -void -RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function, int16_t value) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - int16_t value2 = value - 1500 + _aux_channels[i]->get_radio_trim(); - _aux_channels[i]->set_radio_out(constrain_int16(value2,_aux_channels[i]->get_radio_min(),_aux_channels[i]->get_radio_max())); - _aux_channels[i]->output(); - } - } -} - -/* - set and save the trim value to radio_in for all channels matching - the given function type - */ -void -RC_Channel_aux::set_trim_to_radio_in_for(RC_Channel_aux::Aux_servo_function_t function) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - if (_aux_channels[i]->get_radio_in() != 0) { - _aux_channels[i]->set_radio_trim( _aux_channels[i]->get_radio_in()); - _aux_channels[i]->save_radio_trim(); - } - } - } -} - -/* - set the radio_out value for any channel with the given function to radio_min - */ -void -RC_Channel_aux::set_radio_to_min(RC_Channel_aux::Aux_servo_function_t function) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - _aux_channels[i]->set_radio_out( _aux_channels[i]->get_radio_min()); - _aux_channels[i]->output(); - } - } -} - -/* - set the radio_out value for any channel with the given function to radio_max - */ -void -RC_Channel_aux::set_radio_to_max(RC_Channel_aux::Aux_servo_function_t function) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - _aux_channels[i]->set_radio_out(_aux_channels[i]->get_radio_max()); - _aux_channels[i]->output(); - } - } -} - -/* - set the radio_out value for any channel with the given function to radio_trim - */ -void -RC_Channel_aux::set_radio_to_trim(RC_Channel_aux::Aux_servo_function_t function) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - _aux_channels[i]->set_radio_out( _aux_channels[i]->get_radio_trim()); - _aux_channels[i]->output(); - } - } -} - -/* - copy radio_in to radio_out for a given function - */ -void -RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function, bool do_input_output) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - if (do_input_output) { - _aux_channels[i]->input(); - } - _aux_channels[i]->set_radio_out(_aux_channels[i]->get_radio_in()); - if (do_input_output) { - _aux_channels[i]->output(); - } - } - } -} - -/* - set servo_out and call calc_pwm() for a given function - */ -void -RC_Channel_aux::set_servo_out_for(RC_Channel_aux::Aux_servo_function_t function, int16_t value) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { - _aux_channels[i]->set_servo_out(value); - _aux_channels[i]->calc_pwm(); - _aux_channels[i]->output(); - } - } -} - -/* - setup failsafe value for an auxiliary function type to a LimitValue - */ -void -RC_Channel_aux::set_servo_failsafe_pwm(RC_Channel_aux::Aux_servo_function_t function, uint16_t pwm) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - const RC_Channel_aux *ch = _aux_channels[i]; - if (ch && ch->function.get() == function) { - hal.rcout->set_failsafe_pwm(1U<get_ch_out(), pwm); - } - } -} - -/* - setup failsafe value for an auxiliary function type to a LimitValue - */ -void -RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::Aux_servo_function_t function, RC_Channel::LimitValue limit) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - const RC_Channel_aux *ch = _aux_channels[i]; - if (ch && ch->function.get() == function) { - uint16_t pwm = ch->get_limit_pwm(limit); - hal.rcout->set_failsafe_pwm(1U<get_ch_out(), pwm); - } - } -} - -/* - set radio output value for an auxiliary function type to a LimitValue - */ -void -RC_Channel_aux::set_servo_limit(RC_Channel_aux::Aux_servo_function_t function, RC_Channel::LimitValue limit) -{ - if (!function_assigned(function)) { - return; - } - for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { - RC_Channel_aux *ch = _aux_channels[i]; - if (ch && ch->function.get() == function) { - uint16_t pwm = ch->get_limit_pwm(limit); - ch->set_radio_out(pwm); - if (ch->function.get() == k_manual) { - // in order for output_ch() to work for k_manual we - // also have to override radio_in - ch->set_radio_in(pwm); - } - if (ch->function.get() >= k_rcin1 && ch->function.get() <= k_rcin16) { - // save for k_rcin* - ch->set_radio_in(pwm); - } - } - } -} - -/* - return true if a particular function is assigned to at least one RC channel - */ -bool -RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function) -{ - if (function < k_nr_aux_servo_functions) { - uint8_t fn = (uint8_t)function; - uint8_t idx = fn / 64; - uint8_t bit = fn % 64; - return (_function_mask[idx] & (1ULL<function.get() == function) { - _aux_channels[i]->set_servo_out(value); - _aux_channels[i]->set_range(angle_min, angle_max); - _aux_channels[i]->calc_pwm(); - _aux_channels[i]->output(); - } - } -} - -/* - set the default channel an auxiliary output function should be on - */ -bool RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::Aux_servo_function_t function, uint8_t channel) -{ - if (!_initialised) { - update_aux_servo_function(); - } - if (function_assigned(function)) { - // already assigned - return true; - } - for (uint8_t i=0; i_ch_out == channel) { - if (_aux_channels[i]->function != k_none) { - if (_aux_channels[i]->function == function) { - return true; - } - hal.console->printf("Channel %u already assigned %u\n", - (unsigned)channel, - (unsigned)_aux_channels[i]->function); - return false; - } - _aux_channels[i]->function.set(function); - _aux_channels[i]->aux_servo_function_setup(); - return true; - } - } - hal.console->printf("AUX channel %u not available\n", - (unsigned)channel); - return false; -} - -// find first channel that a function is assigned to -bool RC_Channel_aux::find_channel(RC_Channel_aux::Aux_servo_function_t function, uint8_t &chan) -{ - if (!_initialised) { - update_aux_servo_function(); - } - if (!function_assigned(function)) { - return false; - } - for (uint8_t i=0; ifunction == function) { - chan = _aux_channels[i]->_ch_out; - return true; - } - } - return false; -} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h deleted file mode 100644 index 50465d8f8c..0000000000 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ /dev/null @@ -1,184 +0,0 @@ -/// @file RC_Channel_aux.h -/// @brief RC_Channel manager for auxiliary channels (5..8), with EEPROM-backed storage of constants. -/// @author Amilcar Lucas -#pragma once - -#include -#include "RC_Channel.h" - -#define RC_AUX_MAX_CHANNELS 12 - -/// @class RC_Channel_aux -/// @brief Object managing one aux. RC channel (CH5-8), with information about its function -class RC_Channel_aux : public RC_Channel { -public: - /// Constructor - /// - /// @param key EEPROM storage key for the channel trim parameters. - /// @param name Optional name for the group. - /// - RC_Channel_aux(uint8_t ch_out) : - RC_Channel(ch_out) - { - for (uint8_t i=0; i #include +#include #include "SRV_Channel.h" #include "RC_Channel.h" extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo SRV_Channels::var_info[] = { - // @Param: RNG_ENABLE - // @DisplayName: Enable servo output ranges - // @Description: This enables the use of separate servo output ranges from input ranges. - // @Values: 0:Disable,1:Enable - // @User: Advanced - AP_GROUPINFO_FLAGS("_RNG_ENABLE", 1, SRV_Channels, enable, 0, AP_PARAM_FLAG_ENABLE), +SRV_Channel *SRV_Channels::channels; +bool SRV_Channels::disabled_passthrough; +bool SRV_Channels::initialised; +Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions}; +SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions]; +SRV_Channel::servo_mask_t SRV_Channel::have_pwm_mask; - // @Param: 1_MIN - // @DisplayName: Servo1 min PWM - // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. +const AP_Param::GroupInfo SRV_Channel::var_info[] = { + // @Param: MIN + // @DisplayName: Minimum PWM + // @Description: minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. // @Units: pwm // @Range: 800 2200 // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("1_MIN", 2, SRV_Channels, servo_min[0], 1100), + // @User: Standard + AP_GROUPINFO("MIN", 1, SRV_Channel, servo_min, 1100), - // @Param: 1_MAX - // @DisplayName: Servo1 max PWM - // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. + // @Param: MAX + // @DisplayName: Maximum PWM + // @Description: maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. // @Units: pwm // @Range: 800 2200 // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("1_MAX", 3, SRV_Channels, servo_max[0], 1900), + // @User: Standard + AP_GROUPINFO("MAX", 2, SRV_Channel, servo_max, 1900), - // @Param: 1_TRIM - // @DisplayName: Servo1 trim PWM - // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. + // @Param: TRIM + // @DisplayName: Trim PWM + // @Description: Trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. // @Units: pwm // @Range: 800 2200 // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("1_TRIM", 4, SRV_Channels, servo_trim[0], 1500), + // @User: Standard + AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500), - // @Param: 1_REV - // @DisplayName: Servo1 reverse - // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel. - // @Values: -1:Reversed,1:Normal - // @User: Advanced - AP_GROUPINFO("1_REV", 5, SRV_Channels, reverse[0], 1), + // @Param: REVERSED + // @DisplayName: Servo reverse + // @Description: Reverse servo operation. Set to 0 for normal operation. Set to 1 to reverse this channel. + // @Values: 0:Normal,1:Reversed + // @User: Standard + AP_GROUPINFO("REVERSED", 4, SRV_Channel, reversed, 0), - // @Param: 2_MIN - // @DisplayName: Servo1 min PWM - // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_MIN", 6, SRV_Channels, servo_min[1], 1100), - - // @Param: 2_MAX - // @DisplayName: Servo1 max PWM - // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_MAX", 7, SRV_Channels, servo_max[1], 1900), - - // @Param: 2_TRIM - // @DisplayName: Servo1 trim PWM - // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_TRIM", 8, SRV_Channels, servo_trim[1], 1500), - - // @Param: 2_REV - // @DisplayName: Servo1 reverse - // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel. - // @Values: -1:Reversed,1:Normal - // @User: Advanced - AP_GROUPINFO("2_REV", 9, SRV_Channels, reverse[1], 1), - - // @Param: 3_MIN - // @DisplayName: Servo1 min PWM - // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_MIN", 10, SRV_Channels, servo_min[2], 1100), - - // @Param: 3_MAX - // @DisplayName: Servo1 max PWM - // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_MAX", 11, SRV_Channels, servo_max[2], 1900), - - // @Param: 3_TRIM - // @DisplayName: Servo1 trim PWM - // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("3_TRIM", 12, SRV_Channels, servo_trim[2], 1500), - - // @Param: 3_REV - // @DisplayName: Servo1 reverse - // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel. - // @Values: -1:Reversed,1:Normal - // @User: Advanced - AP_GROUPINFO("3_REV", 13, SRV_Channels, reverse[2], 1), - - // @Param: 4_MIN - // @DisplayName: Servo1 min PWM - // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_MIN", 14, SRV_Channels, servo_min[3], 1100), - - // @Param: 4_MAX - // @DisplayName: Servo1 max PWM - // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_MAX", 15, SRV_Channels, servo_max[3], 1900), - - // @Param: 4_TRIM - // @DisplayName: Servo1 trim PWM - // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. - // @Units: pwm - // @Range: 800 2200 - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("4_TRIM", 16, SRV_Channels, servo_trim[3], 1500), - - // @Param: 4_REV - // @DisplayName: Servo1 reverse - // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel. - // @Values: -1:Reversed,1:Normal - // @User: Advanced - AP_GROUPINFO("4_REV", 17, SRV_Channels, reverse[3], 1), - - // @Param: _AUTO_TRIM - // @DisplayName: Automatic servo trim - // @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights. - // @Values: 0:Disable,1:Enable - // @User: Advanced - AP_GROUPINFO("_AUTO_TRIM", 18, SRV_Channels, auto_trim, 0), + // @Param: FUNCTION + // @DisplayName: Servo output function + // @Description: Function assigned to this servo. Seeing this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function + // @Values: 0:Disabled,1:RCPassThru,2:Flap,3:Flap_auto,4:Aileron,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release,12:mount2_pan,13:mount2_tilt,14:mount2_roll,15:mount2_open,16:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,68:Choke,69:Starter,70:Throttle + // @User: Standard + AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), AP_GROUPEND }; +const AP_Param::GroupInfo SRV_Channels::var_info[] = { + // @Group: 1_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[0], "1_", 1, SRV_Channels, SRV_Channel), + + // @Group: 2_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[1], "2_", 2, SRV_Channels, SRV_Channel), + + // @Group: 3_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[2], "3_", 3, SRV_Channels, SRV_Channel), + + // @Group: 4_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[3], "4_", 4, SRV_Channels, SRV_Channel), + + // @Group: 5_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[4], "5_", 5, SRV_Channels, SRV_Channel), + + // @Group: 6_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[5], "6_", 6, SRV_Channels, SRV_Channel), + + // @Group: 7_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[6], "7_", 7, SRV_Channels, SRV_Channel), + + // @Group: 8_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[7], "8_", 8, SRV_Channels, SRV_Channel), + + // @Group: 9_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[8], "9_", 9, SRV_Channels, SRV_Channel), + + // @Group: 10_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[9], "10_", 10, SRV_Channels, SRV_Channel), + + // @Group: 11_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[10], "11_", 11, SRV_Channels, SRV_Channel), + + // @Group: 12_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[11], "12_", 12, SRV_Channels, SRV_Channel), + + // @Group: 13_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[12], "13_", 13, SRV_Channels, SRV_Channel), + + // @Group: 14_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[13], "14_", 14, SRV_Channels, SRV_Channel), + + // @Group: 15_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[14], "15_", 15, SRV_Channels, SRV_Channel), + + // @Group: 16_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[15], "16_", 16, SRV_Channels, SRV_Channel), + + // @Param: _AUTO_TRIM + // @DisplayName: Automatic servo trim + // @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights. + // @Values: 0:Disable,1:Enable + // @User: Advanced + AP_GROUPINFO("_AUTO_TRIM", 17, SRV_Channels, auto_trim, 0), + + AP_GROUPEND +}; + +SRV_Channel::SRV_Channel(void) +{ + AP_Param::setup_object_defaults(this, var_info); + // start with all pwm at zero + have_pwm_mask = ~uint16_t(0); +} + + /* constructor */ -SRV_Channels::SRV_Channels(void) +SRV_Channels::SRV_Channels(const RCMapper &_rcmap) : + rcmap(_rcmap) { + channels = obj_channels; + + // set defaults from the parameter table AP_Param::setup_object_defaults(this, var_info); -} - -// remap a PWM value from a channel in value -uint16_t SRV_Channels::remap_pwm(uint8_t i, uint16_t pwm) const -{ - const RC_Channel *ch = RC_Channel::rc_channel(i); - if (ch == nullptr) { - return 0; + // setup ch_num on channels + for (uint8_t i=0; iget_radio_out_normalised(pwm); - uint16_t radio_out; - if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) { - if (reverse[i] == -1) { - v = 1 - v; - } - radio_out = servo_min[i] + v * (servo_max[i] - servo_min[i]); - } else { - if (reverse[i] == -1) { - v = -v; - } - if (v > 0) { - radio_out = servo_trim[i] + v * (servo_max[i] - servo_trim[i]); - } else { - radio_out = servo_trim[i] + v * (servo_trim[i] - servo_min[i]); - } - } - radio_out = constrain_int16(radio_out, servo_min[i], servo_max[i]); - return radio_out; -} - -/* - remap radio_out values for first 4 servos using SERVO* parameters, if enabled - This should be called with cork enabled in hal.rcout - */ -void SRV_Channels::remap_servo_output(void) -{ - // cope with SERVO_RNG_ENABLE being changed at runtime. If we - // don't change the ESC scaling immediately then some ESCs will - // fire up for one second - if (last_enable != enable && esc_cal_chan != -1) { - last_enable = enable; - set_esc_scaling(esc_cal_chan); - } - if (!enable) { - return; - } - for (uint8_t i=0; iget_radio_out()); - ch->set_radio_out(radio_out); - } -} - - -/* - set trim values for output channels - */ -void SRV_Channels::set_trim(void) -{ - if (!enable) { - return; - } - for (uint8_t i=0; iget_type_out() == RC_CHANNEL_TYPE_RANGE) { - // we don't trim range channels (like throttle) - continue; - } - uint16_t new_trim = remap_pwm(i, ch->get_radio_trim()); - servo_trim[i].set_and_save(new_trim); - } -} - -void SRV_Channels::set_esc_scaling(uint8_t chnum) -{ - esc_cal_chan = chnum; - if (!enable || chnum >= NUM_SERVO_RANGE_CHANNELS) { - const RC_Channel *ch = RC_Channel::rc_channel(chnum); - hal.rcout->set_esc_scaling(ch->get_radio_min(), ch->get_radio_max()); - } else { - hal.rcout->set_esc_scaling(servo_min[chnum], servo_max[chnum]); - } -} - -/* - auto-adjust channel trim from an integrator value. Positive v means - adjust trim up. Negative means decrease - */ -void SRV_Channels::adjust_trim(uint8_t chnum, float v) -{ - if (reverse[chnum] == -1) { - v = -v; - } - uint16_t new_trim = servo_trim[chnum]; - float trim_scaled = float(servo_trim[chnum] - servo_min[chnum]) / (servo_max[chnum] - servo_min[chnum]); - if (v > 0 && trim_scaled < 0.6f) { - new_trim++; - } else if (v < 0 && trim_scaled > 0.4f) { - new_trim--; - } else { - return; - } - servo_trim[chnum].set(new_trim); - - trimmed_mask |= 1U<= high_out) { + scaled_value = high_out; + } + if (scaled_value < 0) { + scaled_value = 0; + } + if (reversed) { + scaled_value = high_out - scaled_value; + } + return servo_min + ((int32_t)scaled_value * (int32_t)(servo_max - servo_min)) / (int32_t)high_out; +} + +// convert a -angle_max..angle_max to a pwm +uint16_t SRV_Channel::pwm_from_angle(int16_t scaled_value) const +{ + if (reversed) { + scaled_value = -scaled_value; + } + if (scaled_value > 0) { + return servo_trim + ((int32_t)scaled_value * (int32_t)(servo_max - servo_trim)) / (int32_t)high_out; + } else { + return servo_trim - (-(int32_t)scaled_value * (int32_t)(servo_trim - servo_min)) / (int32_t)high_out; + } +} + +void SRV_Channel::calc_pwm(int16_t output_scaled) +{ + if (have_pwm_mask & (1U<set_failsafe_pwm(1U< mid) { + ret = (float)(output_pwm - mid) / (float)(servo_max - mid); + } else { + ret = 0; + } + if (get_reversed()) { + ret = -ret; + } + return ret; +} + +uint16_t SRV_Channel::get_limit_pwm(LimitValue limit) const +{ + switch (limit) { + case SRV_CHANNEL_LIMIT_TRIM: + return servo_trim; + case SRV_CHANNEL_LIMIT_MIN: + return servo_min; + case SRV_CHANNEL_LIMIT_MAX: + return servo_max; + case SRV_CHANNEL_LIMIT_ZERO_PWM: + default: + return 0; + } +} + +/* + run calc_pwm for all channels + */ +void SRV_Channels::calc_pwm(void) +{ + for (uint8_t i=0; i #include +#include +#include + #include "RC_Channel.h" -#define NUM_SERVO_RANGE_CHANNELS 4 +#define NUM_SERVO_CHANNELS 16 + +class SRV_Channels; /* - class SRV_Channel + class SRV_Channel. The class SRV_Channels contains an array of + SRV_Channel objects. This is done to fit within the AP_Param limit + of 64 parameters per object. +*/ +class SRV_Channel { +public: + friend class SRV_Channels; + // constructor + SRV_Channel(void); + + static const struct AP_Param::GroupInfo var_info[]; + + typedef enum + { + k_none = 0, ///< disabled + k_manual = 1, ///< manual, just pass-thru the RC in signal + k_flap = 2, ///< flap + k_flap_auto = 3, ///< flap automated + k_aileron = 4, ///< aileron + k_unused1 = 5, ///< unused function + k_mount_pan = 6, ///< mount yaw (pan) + k_mount_tilt = 7, ///< mount pitch (tilt) + k_mount_roll = 8, ///< mount roll + k_mount_open = 9, ///< mount open (deploy) / close (retract) + k_cam_trigger = 10, ///< camera trigger + k_egg_drop = 11, ///< egg drop + k_mount2_pan = 12, ///< mount2 yaw (pan) + k_mount2_tilt = 13, ///< mount2 pitch (tilt) + k_mount2_roll = 14, ///< mount2 roll + k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) + k_dspoiler1 = 16, ///< differential spoiler 1 (left wing) + k_dspoiler2 = 17, ///< differential spoiler 2 (right wing) + k_aileron_with_input = 18, ///< aileron, with rc input + k_elevator = 19, ///< elevator + k_elevator_with_input = 20, ///< elevator, with rc input + k_rudder = 21, ///< secondary rudder channel + k_sprayer_pump = 22, ///< crop sprayer pump channel + k_sprayer_spinner = 23, ///< crop sprayer spinner channel + k_flaperon1 = 24, ///< flaperon, left wing + k_flaperon2 = 25, ///< flaperon, right wing + k_steering = 26, ///< ground steering, used to separate from rudder + k_parachute_release = 27, ///< parachute release + k_gripper = 28, ///< gripper + k_landing_gear_control = 29, ///< landing gear controller + k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters + k_heli_rsc = 31, ///< helicopter RSC output + k_heli_tail_rsc = 32, ///< helicopter tail RSC output + k_motor1 = 33, ///< these allow remapping of copter motors + k_motor2 = 34, + k_motor3 = 35, + k_motor4 = 36, + k_motor5 = 37, + k_motor6 = 38, + k_motor7 = 39, + k_motor8 = 40, + k_motor_tilt = 41, ///< tiltrotor motor tilt control + k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs + k_rcin2 = 52, + k_rcin3 = 53, + k_rcin4 = 54, + k_rcin5 = 55, + k_rcin6 = 56, + k_rcin7 = 57, + k_rcin8 = 58, + k_rcin9 = 59, + k_rcin10 = 60, + k_rcin11 = 61, + k_rcin12 = 62, + k_rcin13 = 63, + k_rcin14 = 64, + k_rcin15 = 65, + k_rcin16 = 66, + k_ignition = 67, + k_choke = 68, + k_starter = 69, + k_throttle = 70, + k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) + } Aux_servo_function_t; + + // used to get min/max/trim limit value based on reverse + enum LimitValue { + SRV_CHANNEL_LIMIT_TRIM, + SRV_CHANNEL_LIMIT_MIN, + SRV_CHANNEL_LIMIT_MAX, + SRV_CHANNEL_LIMIT_ZERO_PWM + }; + + // a special scaled output value that is recognised as meaning no pwm output + static const int16_t ZERO_PWM = INT16_MIN; + + // set the output value as a pwm value + void set_output_pwm(uint16_t pwm); + + // get the output value as a pwm value + uint16_t get_output_pwm(void) const { return output_pwm; } + + // set angular range of scaled output + void set_angle(int16_t angle); + + // set range of scaled output. Low is always zero + void set_range(uint16_t high); + + // return true if the channel is reversed + bool get_reversed(void) const { + return reversed?true:false; + } + + // set MIN/MAX parameters + void set_output_min(uint16_t pwm) { + servo_min.set(pwm); + } + void set_output_max(uint16_t pwm) { + servo_max.set(pwm); + } + + // get MIN/MAX/TRIM parameters + uint16_t get_output_min(void) const { + return servo_min; + } + uint16_t get_output_max(void) const { + return servo_max; + } + uint16_t get_trim(void) const { + return servo_trim; + } + +private: + AP_Int16 servo_min; + AP_Int16 servo_max; + AP_Int16 servo_trim; + // reversal, following convention that 1 means reversed, 0 means normal + AP_Int8 reversed; + AP_Int8 function; + + // a pending output value as PWM + uint16_t output_pwm; + + // true for angle output type + bool type_angle:1; + + // set_range() or set_angle() has been called + bool type_setup:1; + + // the hal channel number + uint8_t ch_num; + + // high point of angle or range output + uint16_t high_out; + + // convert a 0..range_max to a pwm + uint16_t pwm_from_range(int16_t scaled_value) const; + + // convert a -angle_max..angle_max to a pwm + uint16_t pwm_from_angle(int16_t scaled_value) const; + + // convert a scaled output to a pwm value + void calc_pwm(int16_t output_scaled); + + // output value based on function + void output_ch(void); + + // setup output type and range based on function + void aux_servo_function_setup(void); + + // return PWM for a given limit value + uint16_t get_limit_pwm(LimitValue limit) const; + + // get normalised output from -1 to 1 + float get_output_norm(void); + + // a bitmask type wide enough for NUM_SERVO_CHANNELS + typedef uint16_t servo_mask_t; + + // mask of channels where we have a output_pwm value. Cleared when a + // scaled value is written. + static servo_mask_t have_pwm_mask; +}; + +/* + class SRV_Channels */ class SRV_Channels { public: // constructor - SRV_Channels(void); + SRV_Channels(const RCMapper &_rcmap); static const struct AP_Param::GroupInfo var_info[]; + // set the default function for a channel + static void set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function); + + // set output value for a function channel as a pwm value + static void set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value); + + // set output value for a function channel as a pwm value on the first matching channel + static void set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t value); + + // set output value for a function channel as a scaled value. This + // calls calc_pwm() to also set the pwm value + static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value); + + // get scaled output for the given function type. + static int16_t get_output_scaled(SRV_Channel::Aux_servo_function_t function); + + // get pwm output for the first channel of the given function type. + static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value); + + // get normalised output (-1 to 1 for angle, 0 to 1 for range). Value is taken from pwm value + // return zero on error. + static float get_output_norm(SRV_Channel::Aux_servo_function_t function); + + // limit slew rate to given limit in percent per second + static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate); + + // call output_ch() on all channels + static void output_ch_all(void); + // take current radio_out for first 4 channels and remap using // servo ranges if enabled void remap_servo_output(void); - // set and save trim values from current RC input trim - void set_trim(void); - - // setup output ESC scaling for a channel - void set_esc_scaling(uint8_t chnum); - - // return true when enabled - bool enabled(void) const { return enable; } + // setup output ESC scaling based on a channels MIN/MAX + void set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function); // return true when auto_trim enabled bool auto_trim_enabled(void) const { return auto_trim; } - + // adjust trim of a channel by a small increment - void adjust_trim(uint8_t ch, float v); + void adjust_trim(SRV_Channel::Aux_servo_function_t function, float v); // save trims void save_trim(void); + + // setup for a reversible k_throttle (from -100 to 100) + void set_reversible_throttle(void) { + flags.k_throttle_reversible = true; + } + + // set all outputs to the TRIM value + static void output_trim_all(void); + + // setup IO failsafe for all channels to trim + static void setup_failsafe_trim_all(void); + + // set output for all channels matching the given function type, allow radio_trim to center servo + static void set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value); + + // set and save the trim for a function channel to radio_in on matching input channel + static void set_trim_to_radio_in_for(SRV_Channel::Aux_servo_function_t function); + + // set the trim for a function channel to min of the channel + static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function); + + // set the trim for a function channel to given pwm + static void set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm); + + // set output to min value + static void set_output_to_min(SRV_Channel::Aux_servo_function_t function); + + // set output to max value + static void set_output_to_max(SRV_Channel::Aux_servo_function_t function); + + // set output to trim value + static void set_output_to_trim(SRV_Channel::Aux_servo_function_t function); + + // copy radio_in to radio_out + static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false); + + // setup failsafe for an auxiliary channel function, by pwm + static void set_failsafe_pwm(SRV_Channel::SRV_Channel::Aux_servo_function_t function, uint16_t pwm); + + // setup failsafe for an auxiliary channel function + static void set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // setup safety for an auxiliary channel function (used when disarmed) + static void set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // set servo to a LimitValue + static void set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // return true if a function is assigned to a channel + static bool function_assigned(SRV_Channel::Aux_servo_function_t function); + + // set a servo_out value, and angle range, then calc_pwm + static void move_servo(SRV_Channel::Aux_servo_function_t function, + int16_t value, int16_t angle_min, int16_t angle_max); + + // assign and enable auxiliary channels + static void enable_aux_servos(void); + + // prevent a channel from being used for auxiliary functions + static void disable_aux_channel(uint8_t channel); + + // return the current function for a channel + static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel); + + // refresh aux servo to function mapping + static void update_aux_servo_function(void); + + // set default channel for an auxiliary function + static bool set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel); + + // find first channel that a function is assigned to + static bool find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan); + + // find first channel that a function is assigned to, returning SRV_Channel object + static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan=-1); + + // call set_angle() on matching channels + static void set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle); + + // call set_range() on matching channels + static void set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range); + // control pass-thru of channels + void disable_passthrough(bool disable) { + disabled_passthrough = disable; + } + + static bool passthrough_disabled(void) { + return disabled_passthrough; + } + + // calculate PWM for all channels + static void calc_pwm(void); + + static SRV_Channel *srv_channel(uint8_t i) { + return i= 0 means normal - AP_Int8 reverse[NUM_SERVO_RANGE_CHANNELS]; + uint16_t trimmed_mask; + + static Bitmask function_mask; + static bool initialised; + + // this static arrangement is to avoid having static objects in AP_Param tables + static SRV_Channel *channels; + SRV_Channel obj_channels[NUM_SERVO_CHANNELS]; + + static struct srv_function { + // mask of what channels this applies to + SRV_Channel::servo_mask_t channel_mask; + + // scaled output for this function + int16_t output_scaled; + } functions[SRV_Channel::k_nr_aux_servo_functions]; AP_Int8 auto_trim; - // remap a PWM value from a channel in value - uint16_t remap_pwm(uint8_t ch, uint16_t pwm) const; + // initialise parameters from RC_Channel + void initialise_parameters(void); }; diff --git a/libraries/RC_Channel/SRV_Channel_aux.cpp b/libraries/RC_Channel/SRV_Channel_aux.cpp new file mode 100644 index 0000000000..af9df3fd01 --- /dev/null +++ b/libraries/RC_Channel/SRV_Channel_aux.cpp @@ -0,0 +1,569 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + SRV_Channel_aux.cpp - handling of servo auxillary functions + */ +#include "SRV_Channel.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +/// map a function to a servo channel and output it +void SRV_Channel::output_ch(void) +{ + int8_t passthrough_from = -1; + + // take care of special function cases + switch(function) + { + case k_manual: // manual + passthrough_from = ch_num; + break; + case k_rcin1 ... k_rcin16: // rc pass-thru + passthrough_from = int8_t(function - k_rcin1); + break; + case k_motor1 ... k_motor8: + // handled by AP_Motors::rc_write() + return; + } + if (passthrough_from != -1) { + // we are doing passthrough from input to output for this channel + RC_Channel *rc = RC_Channels::rc_channel(passthrough_from); + if (rc) { + if (SRV_Channels::passthrough_disabled()) { + output_pwm = rc->get_radio_trim(); + } else { + output_pwm = rc->get_radio_in(); + } + } + } + hal.rcout->write(ch_num, output_pwm); +} + +/* + call output_ch() on all channels + */ +void SRV_Channels::output_ch_all(void) +{ + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + channels[i].output_ch(); + } +} + +/* + return the current function for a channel +*/ +SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel) +{ + if (channel < NUM_SERVO_CHANNELS) { + return (SRV_Channel::Aux_servo_function_t)channels[channel].function.get(); + } + return SRV_Channel::k_none; +} + +/* + setup a channels aux servo function +*/ +void SRV_Channel::aux_servo_function_setup(void) +{ + if (type_setup) { + return; + } + switch (function) { + case k_flap: + case k_flap_auto: + case k_egg_drop: + set_range(100); + break; + case k_heli_rsc: + case k_heli_tail_rsc: + case k_motor_tilt: + set_range(1000); + break; + case k_aileron_with_input: + case k_elevator_with_input: + case k_aileron: + case k_elevator: + case k_dspoiler1: + case k_dspoiler2: + case k_rudder: + case k_steering: + case k_flaperon1: + case k_flaperon2: + set_angle(4500); + break; + case k_throttle: + // fixed wing throttle + set_range(100); + break; + default: + break; + } +} + +/// setup the output range types of all functions +void SRV_Channels::update_aux_servo_function(void) +{ + function_mask.clearall(); + + for (uint8_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) { + functions[i].channel_mask = 0; + } + + // set auxiliary ranges + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + channels[i].aux_servo_function_setup(); + function_mask.set((uint8_t)channels[i].function.get()); + functions[channels[i].function.get()].channel_mask |= 1U<enable_ch(channels[i].ch_num); + } + } +} + +/* + set radio_out for all channels matching the given function type + */ +void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value) +{ + if (!function_assigned(function)) { + return; + } + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + if (channels[i].function.get() == function) { + channels[i].set_output_pwm(value); + channels[i].output_ch(); + } + } +} + +/* + set radio_out for all channels matching the given function type + trim the output assuming a 1500 center on the given value + */ +void +SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value) +{ + if (!function_assigned(function)) { + return; + } + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + if (channels[i].function.get() == function) { + int16_t value2 = value - 1500 + channels[i].get_trim(); + channels[i].set_output_pwm(constrain_int16(value2,channels[i].get_output_min(),channels[i].get_output_max())); + channels[i].output_ch(); + } + } +} + +/* + set and save the trim value to radio_in for all channels matching + the given function type + */ +void +SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::Aux_servo_function_t function) +{ + if (!function_assigned(function)) { + return; + } + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + if (channels[i].function.get() == function) { + RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num); + if (rc && rc->get_radio_in() != 0) { + rc->set_radio_trim(rc->get_radio_in()); + rc->save_radio_trim(); + } + } + } +} + +/* + copy radio_in to radio_out for a given function + */ +void +SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output) +{ + if (!function_assigned(function)) { + return; + } + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + if (channels[i].function.get() == function) { + RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num); + if (rc == nullptr) { + continue; + } + if (do_input_output) { + rc->read(); + } + channels[i].set_output_pwm(rc->get_radio_in()); + if (do_input_output) { + channels[i].output_ch(); + } + } + } +} + +/* + setup failsafe value for an auxiliary function type to a LimitValue + */ +void +SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm) +{ + if (!function_assigned(function)) { + return; + } + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + const SRV_Channel &ch = channels[i]; + if (ch.function.get() == function) { + hal.rcout->set_failsafe_pwm(1U<set_failsafe_pwm(1U<set_safety_pwm(1U<set_radio_in(pwm); + } + } + } + } +} + +/* + return true if a particular function is assigned to at least one RC channel + */ +bool +SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function) +{ + return function_mask.get(uint16_t(function)); +} + +/* + set servo_out and angle_min/max, then calc_pwm and output a + value. This is used to move a AP_Mount servo + */ +void +SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function, + int16_t value, int16_t angle_min, int16_t angle_max) +{ + if (!function_assigned(function)) { + return; + } + if (angle_max <= angle_min) { + return; + } + float v = float(value - angle_min) / float(angle_max - angle_min); + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + SRV_Channel &ch = channels[i]; + if (ch.function.get() == function) { + uint16_t pwm = ch.servo_min + v * (ch.servo_max - ch.servo_min); + ch.set_output_pwm(pwm); + } + } +} + +/* + set the default channel an auxiliary output function should be on + */ +bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel) +{ + if (!initialised) { + update_aux_servo_function(); + } + if (function_assigned(function)) { + // already assigned + return true; + } + if (channels[channel].function != SRV_Channel::k_none) { + if (channels[channel].function == function) { + return true; + } + hal.console->printf("Channel %u already assigned %u\n", + (unsigned)channel, + (unsigned)channels[channel].function); + return false; + } + channels[channel].type_setup = false; + channels[channel].function.set(function); + channels[channel].aux_servo_function_setup(); + function_mask.set((uint8_t)function); + return true; +} + +// find first channel that a function is assigned to +bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan) +{ + if (!initialised) { + update_aux_servo_function(); + } + if (!function_assigned(function)) { + return false; + } + for (uint8_t i=0; i= 0) { + set_aux_channel_default(function, default_chan); + } + if (!find_channel(function, chan)) { + return nullptr; + } + return &channels[chan]; +} + +void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value) +{ + if (function < SRV_Channel::k_nr_aux_servo_functions) { + functions[function].output_scaled = value; + SRV_Channel::have_pwm_mask &= ~functions[function].channel_mask; + } +} + +int16_t SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function) +{ + if (function < SRV_Channel::k_nr_aux_servo_functions) { + return functions[function].output_scaled; + } + return 0; +} + + +// set the trim for a function channel to given pwm +void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm) +{ + for (uint8_t i=0; iset_esc_scaling(channels[chan].get_output_min(), channels[chan].get_output_max()); + } +} + +/* + auto-adjust channel trim from an integrator value. Positive v means + adjust trim up. Negative means decrease + */ +void SRV_Channels::adjust_trim(SRV_Channel::Aux_servo_function_t function, float v) +{ + if (is_zero(v)) { + return; + } + for (uint8_t i=0; i 0 && trim_scaled < 0.6f) { + new_trim++; + } else if (change < 0 && trim_scaled > 0.4f) { + new_trim--; + } else { + return; + } + c.servo_trim.set(new_trim); + + trimmed_mask |= 1U< #include @@ -18,68 +10,54 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #define NUM_CHANNELS 8 -static RC_Channel rc_1(CH_1); -static RC_Channel rc_2(CH_2); -static RC_Channel rc_3(CH_3); -static RC_Channel rc_4(CH_4); -static RC_Channel rc_5(CH_5); -static RC_Channel rc_6(CH_6); -static RC_Channel rc_7(CH_7); -static RC_Channel rc_8(CH_8); -static RC_Channel **rc = RC_Channel::rc_channel_array(); +static RC_Channels rc_channels; +static RC_Channel *rc; static void print_pwm(void); static void print_radio_values(); -static void copy_input_output(void); void setup() { hal.console->println("ArduPilot RC Channel test"); + rc = RC_Channels::rc_channel(CH_1); + print_radio_values(); // set type of output, symmetrical angles or a number range; - rc_1.set_angle(4500); - rc_1.set_default_dead_zone(80); - rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + rc[CH_1].set_angle(4500); + rc[CH_1].set_default_dead_zone(80); - rc_2.set_angle(4500); - rc_2.set_default_dead_zone(80); - rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + rc[CH_2].set_angle(4500); + rc[CH_2].set_default_dead_zone(80); - rc_3.set_range(0,1000); - rc_3.set_default_dead_zone(20); + rc[CH_3].set_range(1000); + rc[CH_3].set_default_dead_zone(20); - rc_4.set_angle(6000); - rc_4.set_default_dead_zone(500); - rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + rc[CH_4].set_angle(6000); + rc[CH_4].set_default_dead_zone(500); - rc_5.set_range(0,1000); - rc_6.set_range(200,800); + rc[CH_5].set_range(1000); + rc[CH_6].set_range(800); - rc_7.set_range(0,1000); + rc[CH_7].set_range(1000); - rc_8.set_range(0,1000); - for (int i=0; ienable_out(); - } + rc[CH_8].set_range(1000); } void loop() { - RC_Channel::set_pwm_all(); + RC_Channels::set_pwm_all(); print_pwm(); - copy_input_output(); - hal.scheduler->delay(20); } static void print_pwm(void) { for (int i=0; iprintf("ch%u: %4d ", (unsigned)i+1, (int)rc[i]->get_control_in()); + hal.console->printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].get_control_in()); } hal.console->printf("\n"); } @@ -90,22 +68,10 @@ static void print_radio_values() for (int i=0; iprintf("CH%u: %u|%u\n", (unsigned)i+1, - (unsigned)rc[i]->get_radio_min(), - (unsigned)rc[i]->get_radio_max()); + (unsigned)rc[i].get_radio_min(), + (unsigned)rc[i].get_radio_max()); } } -/* - copy scaled input to output - */ -static void copy_input_output(void) -{ - for (int i=0; iset_servo_out(rc[i]->get_control_in()); - rc[i]->calc_pwm(); - rc[i]->output(); - } -} - AP_HAL_MAIN(); diff --git a/libraries/RC_Channel/examples/RC_UART/RC_UART.cpp b/libraries/RC_Channel/examples/RC_UART/RC_UART.cpp index e5eff5509f..6fc7311e88 100644 --- a/libraries/RC_Channel/examples/RC_UART/RC_UART.cpp +++ b/libraries/RC_Channel/examples/RC_UART/RC_UART.cpp @@ -3,7 +3,6 @@ */ #include -#include #include const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -24,12 +23,6 @@ private: uint8_t enable_mask; const uint32_t baudrate = 115200; uint32_t counter; - - RC_Channel rc_1{0}; - RC_Channel rc_2{1}; - RC_Channel rc_3{2}; - RC_Channel rc_4{3}; - RC_Channel *rc = &rc_1; }; void RC_UART::setup() @@ -94,11 +87,10 @@ void RC_UART::loop() if (enable_mask == 0) { hal.rcout->force_safety_off(); } - rc[i].enable_out(); + hal.rcout->enable_ch(i); enable_mask |= 1U<write(i, u.period[i]); } // report periods to console for debug