Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

RC_Channel: 2nd stage of SRV_Channels conversion

Browse Source 
allow for 16 output channel objects
This commit is contained in:
Andrew Tridgell 2016-10-22 21:27:40 +11:00
parent 62fabca19d
commit 64aa13e7f4
9 changed files with 1440 additions and 1501 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

551
libraries/RC_Channel/RC_Channel.cpp
View File

@ -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; i<NUM_RC_CHANNELS; i++) {
channels[i].ch_in = i;
}
}
void
RC_Channel::set_range_out(int16_t low, int16_t high)
RC_Channel::set_range(uint16_t high)
{
_type_out = RC_CHANNEL_TYPE_RANGE;
_high_out = high;
_low_out = low;
type_in = RC_CHANNEL_TYPE_RANGE;
high_in = high;
}
void
RC_Channel::set_range_in(int16_t low, int16_t high)
RC_Channel::set_angle(uint16_t angle)
{
_type_in = RC_CHANNEL_TYPE_RANGE;
_high_in = high;
_low_in = low;
}
void
RC_Channel::set_angle(int16_t angle)
{
set_angle_in(angle);
set_angle_out(angle);
}
void
RC_Channel::set_angle_out(int16_t angle)
{
_type_out = RC_CHANNEL_TYPE_ANGLE;
_high_out = angle;
}
void
RC_Channel::set_angle_in(int16_t angle)
{
_type_in = RC_CHANNEL_TYPE_ANGLE;
_high_in = angle;
type_in = RC_CHANNEL_TYPE_ANGLE;
high_in = angle;
}
void
RC_Channel::set_default_dead_zone(int16_t dzone)
{
_dead_zone.set_default(abs(dzone));
}
void
RC_Channel::set_reverse(bool reverse)
{
if (reverse) _reverse = -1;
else _reverse = 1;
dead_zone.set_default(abs(dzone));
}
bool
RC_Channel::get_reverse(void) const
{
if (_reverse == -1) {
return true;
}
return false;
}
void
RC_Channel::set_type(uint8_t t)
{
set_type_in(t);
set_type_out(t);
}
void
RC_Channel::set_type_in(uint8_t t)
{
_type_in = t;
}
void
RC_Channel::set_type_out(uint8_t t)
{
_type_out = t;
}
// call after first read
void
RC_Channel::trim()
{
_radio_trim = _radio_in;
return bool(reversed.get());
}
// 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();
if (type_in == RC_CHANNEL_TYPE_RANGE) {
control_in = pwm_to_range();
} else {
//RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
_control_in = pwm_to_angle();
//RC_CHANNEL_TYPE_ANGLE
control_in = pwm_to_angle();
}
}
@ -195,12 +213,10 @@ RC_Channel::set_pwm(int16_t pwm)
call read() and set_pwm() on all channels
*/
void
RC_Channel::set_pwm_all(void)
RC_Channels::set_pwm_all(void)
{
for (uint8_t i=0; i<RC_MAX_CHANNELS; i++) {
if (_rc_ch[i] != nullptr) {
_rc_ch[i]->set_pwm(_rc_ch[i]->read());
}
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
channels[i].set_pwm(channels[i].read());
}
}
@ -210,54 +226,32 @@ 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);
if (type_in == RC_CHANNEL_TYPE_RANGE) {
control_in = pwm_to_range_dz(0);
} else {
//RC_CHANNEL_ANGLE, RC_CHANNEL_ANGLE_RAW
_control_in = pwm_to_angle_dz(0);
//RC_CHANNEL_ANGLE
control_in = pwm_to_angle_dz(0);
}
}
// returns just the PWM without the offset from radio_min
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);
}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; i<RC_MAX_CHANNELS; i++) {
if (_rc_ch[i] != nullptr) {
_rc_ch[i]->output_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; i<RC_MAX_CHANNELS; i++) {
if (_rc_ch[i] != nullptr && ((1U<<i)&chmask)) {
hal.rcout->set_failsafe_pwm(1U<<i, _rc_ch[i]->_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;
}

169
libraries/RC_Channel/RC_Channel.h
View File

@ -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; }
// get the channel number
uint8_t get_ch_out(void) const { return _ch_out; }
uint16_t get_dead_zone(void) const { return dead_zone; }
// 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 RC_Channel *rc_channel(uint8_t i);
static RC_Channel **rc_channel_array(void)
{
return _rc_ch;
}
static const struct AP_Param::GroupInfo var_info[];
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;}
// get control input with zero deadzone
int16_t get_control_in_zero_dz(void);
int16_t get_pwm_out() const { return _pwm_out;}
int16_t get_radio_min() const {return radio_min.get();}
void set_radio_min(int16_t val) { radio_min = val;}
int16_t get_radio_out() const { return _radio_out;}
void set_radio_out(int16_t val){ _radio_out = 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_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;
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_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_Int16 radio_min;
AP_Int16 radio_trim;
AP_Int16 radio_max;
static RC_Channel *_rc_ch[RC_MAX_CHANNELS];
AP_Int8 reversed;
AP_Int16 dead_zone;
protected:
uint8_t _ch_out;
uint8_t type_in;
int16_t high_in;
// 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];
};

505
libraries/RC_Channel/RC_Channel_aux.cpp
View File

@ -1,505 +0,0 @@
#include "RC_Channel_aux.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
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<<ch->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<<ch->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<<bit)) != 0;
}
return false;
}
/*
set servo_out and angle_min/max, then calc_pwm and output a
value. This is used to move a AP_Mount servo
*/
void
RC_Channel_aux::move_servo(RC_Channel_aux::Aux_servo_function_t function,
int16_t value, int16_t angle_min, int16_t angle_max)
{
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]->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<RC_AUX_MAX_CHANNELS; i++) {
if (_aux_channels[i] && _aux_channels[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; i<RC_AUX_MAX_CHANNELS; i++) {
if (_aux_channels[i] && _aux_channels[i]->function == function) {
chan = _aux_channels[i]->_ch_out;
return true;
}
}
return false;
}

184
libraries/RC_Channel/RC_Channel_aux.h
View File

@ -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 <AP_HAL/AP_HAL.h>
#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<RC_AUX_MAX_CHANNELS; i++) {
if (_aux_channels[i] == nullptr) {
_aux_channels[i] = this;
break;
}
}
AP_Param::setup_object_defaults(this, 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;
AP_Int8 function; ///< see Aux_servo_function_t enum
// output one auxiliary channel
void output_ch(void);
// output all auxiliary channels
static void output_ch_all(void);
// set radio_out for a function channel
static void set_radio(Aux_servo_function_t function, int16_t value);
// get radio_out for *first* channel matching the given function type.
static bool get_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t &value);
// set radio_out for all channels matching the given function type, allow radio_trim to center servo
static void set_radio_trimmed(Aux_servo_function_t function, int16_t value);
// set and save the trim for a function channel to radio_in
static void set_trim_to_radio_in_for(Aux_servo_function_t function);
// set radio_out to radio_min
static void set_radio_to_min(Aux_servo_function_t function);
// set radio_out to radio_max
static void set_radio_to_max(Aux_servo_function_t function);
// set radio_out to radio_trim
static void set_radio_to_trim(Aux_servo_function_t function);
// copy radio_in to radio_out
static void copy_radio_in_out(Aux_servo_function_t function, bool do_input_output=false);
// set servo_out
static void set_servo_out_for(Aux_servo_function_t function, int16_t value);
// setup failsafe for an auxiliary channel function, by pwm
static void set_servo_failsafe_pwm(RC_Channel_aux::Aux_servo_function_t function, uint16_t pwm);
// setup failsafe for an auxiliary channel function
static void set_servo_failsafe(Aux_servo_function_t function, RC_Channel::LimitValue limit);
// set servo to a LimitValue
static void set_servo_limit(Aux_servo_function_t function, RC_Channel::LimitValue limit);
// return true if a function is assigned to a channel
static bool function_assigned(Aux_servo_function_t function);
// set a servo_out value, and angle range, then calc_pwm
static void move_servo(Aux_servo_function_t function,
int16_t value, int16_t angle_min, int16_t angle_max);
static const struct AP_Param::GroupInfo var_info[];
// assigned 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 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(Aux_servo_function_t function, uint8_t channel);
// find first channel that a function is assigned to
static bool find_channel(Aux_servo_function_t function, uint8_t &chan);
// control pass-thru of channels
static void disable_passthrough(bool disable) {
_disable_passthrough = disable;
}
private:
static uint64_t _function_mask[2];
static bool _initialised;
static RC_Channel_aux *_aux_channels[RC_AUX_MAX_CHANNELS];
static bool _disable_passthrough;
void aux_servo_function_setup(void);
static void set_function_mask(uint8_t function);
static void clear_function_mask(void);
};

490
libraries/RC_Channel/SRV_Channel.cpp
View File

@ -19,284 +19,163 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include "SRV_Channel.h"
#include "RC_Channel.h"
extern const AP_HAL::HAL& hal;
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;
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: Standard
AP_GROUPINFO("MIN", 1, SRV_Channel, servo_min, 1100),
// @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: Standard
AP_GROUPINFO("MAX", 2, SRV_Channel, servo_max, 1900),
// @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: Standard
AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500),
// @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: 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[] = {
// @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),
// @Group: 1_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[0], "1_", 1, SRV_Channels, SRV_Channel),
// @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.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("1_MIN", 2, SRV_Channels, servo_min[0], 1100),
// @Group: 2_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[1], "2_", 2, SRV_Channels, SRV_Channel),
// @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.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("1_MAX", 3, SRV_Channels, servo_max[0], 1900),
// @Group: 3_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[2], "3_", 3, SRV_Channels, SRV_Channel),
// @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.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("1_TRIM", 4, SRV_Channels, servo_trim[0], 1500),
// @Group: 4_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[3], "4_", 4, SRV_Channels, SRV_Channel),
// @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),
// @Group: 5_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[4], "5_", 5, SRV_Channels, SRV_Channel),
// @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),
// @Group: 6_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[5], "6_", 6, SRV_Channels, SRV_Channel),
// @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),
// @Group: 7_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[6], "7_", 7, SRV_Channels, SRV_Channel),
// @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),
// @Group: 8_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[7], "8_", 8, SRV_Channels, SRV_Channel),
// @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),
// @Group: 9_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[8], "9_", 9, SRV_Channels, SRV_Channel),
// @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),
// @Group: 10_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[9], "10_", 10, SRV_Channels, SRV_Channel),
// @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),
// @Group: 11_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[10], "11_", 11, SRV_Channels, SRV_Channel),
// @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),
// @Group: 12_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[11], "12_", 12, SRV_Channels, SRV_Channel),
// @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),
// @Group: 13_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[12], "13_", 13, SRV_Channels, SRV_Channel),
// @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),
// @Group: 14_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[13], "14_", 14, SRV_Channels, SRV_Channel),
// @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),
// @Group: 15_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[14], "15_", 15, SRV_Channels, SRV_Channel),
// @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),
// @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", 18, SRV_Channels, auto_trim, 0),
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; i<NUM_SERVO_CHANNELS; i++) {
channels[i].ch_num = i;
}
float v = ch->get_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; i<NUM_SERVO_RANGE_CHANNELS; i++) {
RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == nullptr) {
continue;
}
uint16_t radio_out = remap_pwm(i, ch->get_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; i<NUM_SERVO_RANGE_CHANNELS; i++) {
const RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == nullptr) {
continue;
}
if (ch->get_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<<chnum;
}
/*
@ -304,10 +183,139 @@ void SRV_Channels::adjust_trim(uint8_t chnum, float v)
*/
void SRV_Channels::save_trim(void)
{
for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (trimmed_mask & (1U<<i)) {
servo_trim[i].set_and_save(servo_trim[i].get());
channels[i].servo_trim.set_and_save(channels[i].servo_trim.get());
}
}
trimmed_mask = 0;
}
// convert a 0..range_max to a pwm
uint16_t SRV_Channel::pwm_from_range(int16_t scaled_value) const
{
if (servo_max <= servo_min || high_out == 0) {
return servo_min;
}
if (scaled_value >= 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<<ch_num)) {
return;
}
uint16_t pwm;
if (type_angle) {
pwm = pwm_from_angle(output_scaled);
} else {
pwm = pwm_from_range(output_scaled);
}
set_output_pwm(pwm);
}
void SRV_Channels::output_trim_all(void)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].set_output_pwm(channels[i].servo_trim);
}
}
void SRV_Channels::setup_failsafe_trim_all(void)
{
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
hal.rcout->set_failsafe_pwm(1U<<channels[i].ch_num, channels[i].servo_trim);
}
}
void SRV_Channel::set_output_pwm(uint16_t pwm)
{
output_pwm = pwm;
have_pwm_mask |= (1U<<ch_num);
}
// set angular range of scaled output
void SRV_Channel::set_angle(int16_t angle)
{
type_angle = true;
high_out = angle;
type_setup = true;
}
// set range of scaled output
void SRV_Channel::set_range(uint16_t high)
{
type_angle = false;
high_out = high;
type_setup = true;
}
/*
get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
*/
float SRV_Channel::get_output_norm(void)
{
uint16_t mid = (servo_max + servo_min) / 2;
float ret;
if (mid <= servo_min) {
return 0;
}
if (output_pwm < mid) {
ret = (float)(output_pwm - mid) / (float)(mid - servo_min);
} else if (output_pwm > 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<NUM_SERVO_CHANNELS; i++) {
channels[i].calc_pwm(functions[channels[i].function].output_scaled);
}
}

363
libraries/RC_Channel/SRV_Channel.h
View File

@ -15,59 +15,374 @@
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_Common/Bitmask.h>
#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<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
}
private:
AP_Int8 enable;
const RCMapper &rcmap;
int8_t esc_cal_chan;
bool last_enable;
uint8_t trimmed_mask;
struct {
bool k_throttle_reversible:1;
} flags;
// PWM values for min/max and trim
AP_Int16 servo_min[NUM_SERVO_RANGE_CHANNELS];
AP_Int16 servo_max[NUM_SERVO_RANGE_CHANNELS];
AP_Int16 servo_trim[NUM_SERVO_RANGE_CHANNELS];
static bool disabled_passthrough;
// reversal, following convention that < 0 means reversed, >= 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);
};

569
libraries/RC_Channel/SRV_Channel_aux.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
SRV_Channel_aux.cpp - handling of servo auxillary functions
*/
#include "SRV_Channel.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
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<<i;
}
initialised = true;
}
/// Should be called after the the servo functions have been initialized
void SRV_Channels::enable_aux_servos()
{
update_aux_servo_function();
// enable all channels that are 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 < NUM_SERVO_CHANNELS; i++) {
SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)channels[i].function.get();
// see if it is a valid function
if (function < SRV_Channel::k_nr_aux_servo_functions) {
hal.rcout->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<<ch.ch_num, pwm);
}
}
}
/*
setup failsafe value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
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) {
uint16_t pwm = ch.get_limit_pwm(limit);
hal.rcout->set_failsafe_pwm(1U<<ch.ch_num, pwm);
}
}
}
/*
setup safety value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
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) {
uint16_t pwm = ch.get_limit_pwm(limit);
hal.rcout->set_safety_pwm(1U<<ch.ch_num, pwm);
}
}
}
/*
set radio output value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
uint16_t pwm = ch.get_limit_pwm(limit);
ch.set_output_pwm(pwm);
if (ch.function.get() == SRV_Channel::k_manual) {
RC_Channel *rc = RC_Channels::rc_channel(ch.ch_num);
if (rc != nullptr) {
// in order for output_ch() to work for k_manual we
// also have to override radio_in
rc->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<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
chan = channels[i].ch_num;
return true;
}
}
return false;
}
/*
get a pointer to first auxillary channel for a channel function
*/
SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan)
{
uint8_t chan;
if (default_chan >= 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; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].servo_trim.set(pwm);
}
}
}
// set the trim for a function channel to min output
void SRV_Channels::set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].servo_trim.set(channels[i].get_reversed()?channels[i].servo_max:channels[i].servo_min);
}
}
}
/*
set the default function for a channel
*/
void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function)
{
if (chan < NUM_SERVO_CHANNELS) {
channels[chan].function.set_default((uint8_t)function);
}
}
void SRV_Channels::set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function)
{
uint8_t chan;
if (find_channel(function, chan)) {
hal.rcout->set_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<NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i];
if (function != (SRV_Channel::Aux_servo_function_t)(c.function.get())) {
continue;
}
float change = c.reversed?-v:v;
uint16_t new_trim = c.servo_trim;
float trim_scaled = float(c.servo_trim - c.servo_min) / (c.servo_max - c.servo_min);
if (change > 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<<i;
}
}
// get pwm output for the first channel of the given function type.
bool SRV_Channels::get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value)
{
uint8_t chan;
if (!find_channel(function, chan)) {
return false;
}
channels[chan].calc_pwm(functions[function].output_scaled);
value = channels[chan].output_pwm;
return true;
}
// set output pwm to trim for the given function
void SRV_Channels::set_output_to_trim(SRV_Channel::Aux_servo_function_t function)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_output_pwm(channels[i].servo_trim);
}
}
}
// set output pwm to for first matching channel
void SRV_Channels::set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t pwm)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_output_pwm(pwm);
break;
}
}
}
/*
get the normalised output for a channel function from the pwm value
of the first matching channel
*/
float SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t function)
{
uint8_t chan;
if (!find_channel(function, chan)) {
return 0;
}
channels[chan].calc_pwm(functions[function].output_scaled);
return channels[chan].get_output_norm();
}
/*
limit slew rate for an output function to given rate in percent per second
*/
void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate)
{
// NOT IMPLEMENTED YET
}
// call set_angle() on matching channels
void SRV_Channels::set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_angle(angle);
}
}
}
// call set_range() on matching channels
void SRV_Channels::set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_range(range);
}
}
}

74
libraries/RC_Channel/examples/RC_Channel/RC_Channel.cpp
View File

@ -2,14 +2,6 @@
* Example of RC_Channel library.
* Based on original sketch by Jason Short. 2010
*/
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#include <AP_HAL/AP_HAL.h>
#include <RC_Channel/RC_Channel.h>
@ -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; i<NUM_CHANNELS; i++) {
rc[i]->enable_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; i<NUM_CHANNELS; i++) {
hal.console->printf("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; i<NUM_CHANNELS; i++) {
hal.console->printf("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; i<NUM_CHANNELS; i++) {
rc[i]->set_servo_out(rc[i]->get_control_in());
rc[i]->calc_pwm();
rc[i]->output();
}
}
AP_HAL_MAIN();

12
libraries/RC_Channel/examples/RC_UART/RC_UART.cpp
View File

@ -3,7 +3,6 @@
*/
#include <AP_HAL/AP_HAL.h>
#include <RC_Channel/RC_Channel.h>
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
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<<i;
}
rc[i].set_radio_out(u.period[i]);
rc[i].output();
hal.rcout->write(i, u.period[i]);
}
// report periods to console for debug