mirror of https://github.com/ArduPilot/ardupilot
RC_Channel: added setup_failsafe_trim_mask()
This commit is contained in:
parent
8fd31111ad
commit
934d2b6ae4
|
@ -496,16 +496,24 @@ void RC_Channel::output_trim_all()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
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] != NULL && ((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
|
setup the failsafe value to the trim value for all channels
|
||||||
*/
|
*/
|
||||||
void RC_Channel::setup_failsafe_trim_all()
|
void RC_Channel::setup_failsafe_trim_all()
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<RC_MAX_CHANNELS; i++) {
|
setup_failsafe_trim_mask(0xFFFF);
|
||||||
if (_rc_ch[i] != NULL) {
|
|
||||||
hal.rcout->set_failsafe_pwm(1U<<i, _rc_ch[i]->_radio_trim);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
|
@ -108,6 +108,7 @@ public:
|
||||||
void output() const;
|
void output() const;
|
||||||
void output_trim() const;
|
void output_trim() const;
|
||||||
static void output_trim_all();
|
static void output_trim_all();
|
||||||
|
static void setup_failsafe_trim_mask(uint16_t chmask);
|
||||||
static void setup_failsafe_trim_all();
|
static void setup_failsafe_trim_all();
|
||||||
uint16_t read() const;
|
uint16_t read() const;
|
||||||
void input();
|
void input();
|
||||||
|
|
Loading…
Reference in New Issue