mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-10 09:53:58 -04:00
RC_Channel: Allow an input to be ignored
This commit is contained in:
parent
37f7cc4bcf
commit
60f19bbf02
@ -108,14 +108,16 @@ RC_Channel::get_reverse(void) const
|
|||||||
return bool(reversed.get());
|
return bool(reversed.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
// read input from APM_RC - create a control_in value
|
// read input from hal.rcin or overrides
|
||||||
void
|
bool
|
||||||
RC_Channel::set_pwm(int16_t pwm)
|
RC_Channel::update(void)
|
||||||
{
|
{
|
||||||
if (has_override()) {
|
if (has_override() && !(*RC_Channels::options & RC_IGNORE_OVERRIDES)) {
|
||||||
radio_in = override_value;
|
radio_in = override_value;
|
||||||
|
} else if (!(*RC_Channels::options & RC_IGNORE_RECEIVER)) {
|
||||||
|
radio_in = hal.rcin->read(ch_in);
|
||||||
} else {
|
} else {
|
||||||
radio_in = pwm;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (type_in == RC_CHANNEL_TYPE_RANGE) {
|
if (type_in == RC_CHANNEL_TYPE_RANGE) {
|
||||||
@ -124,6 +126,8 @@ RC_Channel::set_pwm(int16_t pwm)
|
|||||||
//RC_CHANNEL_TYPE_ANGLE
|
//RC_CHANNEL_TYPE_ANGLE
|
||||||
control_in = pwm_to_angle();
|
control_in = pwm_to_angle();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// recompute control values with no deadzone
|
// recompute control values with no deadzone
|
||||||
@ -299,12 +303,6 @@ RC_Channel::percent_input()
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t
|
|
||||||
RC_Channel::read() const
|
|
||||||
{
|
|
||||||
return hal.rcin->read(ch_in);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Return true if the channel is at trim and within the DZ
|
Return true if the channel is at trim and within the DZ
|
||||||
*/
|
*/
|
||||||
|
@ -26,6 +26,11 @@ public:
|
|||||||
RC_CHANNEL_LIMIT_MAX
|
RC_CHANNEL_LIMIT_MAX
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum InputIgnore {
|
||||||
|
RC_IGNORE_RECEIVER = (1 << 0), // RC reciever modules
|
||||||
|
RC_IGNORE_OVERRIDES = (1 << 1), // MAVLink overrides
|
||||||
|
};
|
||||||
|
|
||||||
// setup the control preferences
|
// setup the control preferences
|
||||||
void set_range(uint16_t high);
|
void set_range(uint16_t high);
|
||||||
void set_angle(uint16_t angle);
|
void set_angle(uint16_t angle);
|
||||||
@ -37,7 +42,7 @@ public:
|
|||||||
int16_t get_control_mid() const;
|
int16_t get_control_mid() const;
|
||||||
|
|
||||||
// read input from hal.rcin - create a control_in value
|
// read input from hal.rcin - create a control_in value
|
||||||
void set_pwm(int16_t pwm);
|
bool update(void);
|
||||||
void recompute_pwm_no_deadzone();
|
void recompute_pwm_no_deadzone();
|
||||||
|
|
||||||
// calculate an angle given dead_zone and trim. This is used by the quadplane code
|
// calculate an angle given dead_zone and trim. This is used by the quadplane code
|
||||||
@ -60,9 +65,6 @@ public:
|
|||||||
int16_t pwm_to_range();
|
int16_t pwm_to_range();
|
||||||
int16_t pwm_to_range_dz(uint16_t dead_zone);
|
int16_t pwm_to_range_dz(uint16_t dead_zone);
|
||||||
|
|
||||||
// read the input value from hal.rcin for this channel
|
|
||||||
uint16_t read() const;
|
|
||||||
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
// return true if input is within deadzone of trim
|
// return true if input is within deadzone of trim
|
||||||
@ -164,6 +166,8 @@ private:
|
|||||||
static RC_Channel *channels;
|
static RC_Channel *channels;
|
||||||
static bool has_new_overrides;
|
static bool has_new_overrides;
|
||||||
static AP_Float *override_timeout;
|
static AP_Float *override_timeout;
|
||||||
|
static AP_Int32 *options;
|
||||||
RC_Channel obj_channels[NUM_RC_CHANNELS];
|
RC_Channel obj_channels[NUM_RC_CHANNELS];
|
||||||
AP_Float _override_timeout;
|
AP_Float _override_timeout;
|
||||||
|
AP_Int32 _options;
|
||||||
};
|
};
|
||||||
|
@ -31,6 +31,7 @@ extern const AP_HAL::HAL& hal;
|
|||||||
RC_Channel *RC_Channels::channels;
|
RC_Channel *RC_Channels::channels;
|
||||||
bool RC_Channels::has_new_overrides;
|
bool RC_Channels::has_new_overrides;
|
||||||
AP_Float *RC_Channels::override_timeout;
|
AP_Float *RC_Channels::override_timeout;
|
||||||
|
AP_Int32 *RC_Channels::options;
|
||||||
|
|
||||||
const AP_Param::GroupInfo RC_Channels::var_info[] = {
|
const AP_Param::GroupInfo RC_Channels::var_info[] = {
|
||||||
// @Group: 1_
|
// @Group: 1_
|
||||||
@ -105,6 +106,13 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = {
|
|||||||
// @Units: s
|
// @Units: s
|
||||||
AP_GROUPINFO("_OVERRIDE_TIME", 32, RC_Channels, _override_timeout, 3.0),
|
AP_GROUPINFO("_OVERRIDE_TIME", 32, RC_Channels, _override_timeout, 3.0),
|
||||||
|
|
||||||
|
// @Param: _OPTIONS
|
||||||
|
// @DisplayName: RC options
|
||||||
|
// @Description: RC input options
|
||||||
|
// @User: Advanced
|
||||||
|
// @Bitmask: 0:Ignore RC Reciever, 1:Ignore MAVLink Overrides
|
||||||
|
AP_GROUPINFO("_OPTIONS", 33, RC_Channels, _options, 0),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -117,6 +125,7 @@ RC_Channels::RC_Channels(void)
|
|||||||
channels = obj_channels;
|
channels = obj_channels;
|
||||||
|
|
||||||
override_timeout = &_override_timeout;
|
override_timeout = &_override_timeout;
|
||||||
|
options = &_options;
|
||||||
|
|
||||||
// set defaults from the parameter table
|
// set defaults from the parameter table
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
@ -150,9 +159,7 @@ uint8_t RC_Channels::get_radio_in(uint16_t *chans, const uint8_t num_channels)
|
|||||||
return read_channels;
|
return read_channels;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
// update all the input channels
|
||||||
call read() and set_pwm() on all channels if there is new data
|
|
||||||
*/
|
|
||||||
bool
|
bool
|
||||||
RC_Channels::read_input(void)
|
RC_Channels::read_input(void)
|
||||||
{
|
{
|
||||||
@ -162,11 +169,12 @@ RC_Channels::read_input(void)
|
|||||||
|
|
||||||
has_new_overrides = false;
|
has_new_overrides = false;
|
||||||
|
|
||||||
|
bool success = false;
|
||||||
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
|
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
|
||||||
channels[i].set_pwm(channels[i].read());
|
success |= channels[i].update();
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t RC_Channels::get_valid_channel_count(void)
|
uint8_t RC_Channels::get_valid_channel_count(void)
|
||||||
|
Loading…
Reference in New Issue
Block a user