Browse Source

RC_Channel: added disable_aux_channel()

this will be used by copter to ensure motor channels are not enabled
for aux functions
mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
00d2d5946c
  1. 4
      libraries/RC_Channel/RC_Channel.h
  2. 14
      libraries/RC_Channel/RC_Channel_aux.cpp
  3. 3
      libraries/RC_Channel/RC_Channel_aux.h

4
libraries/RC_Channel/RC_Channel.h

@ -118,9 +118,11 @@ private: @@ -118,9 +118,11 @@ private:
int16_t _low;
int16_t _high_out;
int16_t _low_out;
uint8_t _ch_out;
static RC_Channel *rc_ch[RC_MAX_CHANNELS];
protected:
uint8_t _ch_out;
};
// This is ugly, but it fixes poorly architected library

14
libraries/RC_Channel/RC_Channel_aux.cpp

@ -38,6 +38,20 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) @@ -38,6 +38,20 @@ RC_Channel_aux::output_ch(unsigned char ch_nr)
hal.rcout->write(ch_nr, radio_out);
}
/*
prevent a channel from being used for auxillary functions
This is used by the copter code to ensure channels used for motors
can't be used for auxillary 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] = NULL;
}
}
}
/// 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

3
libraries/RC_Channel/RC_Channel_aux.h

@ -99,6 +99,9 @@ public: @@ -99,6 +99,9 @@ public:
// assigned and enable auxillary channels
static void enable_aux_servos(void);
// prevent a channel from being used for auxillary functions
static void disable_aux_channel(uint8_t channel);
private:
static uint32_t _function_mask;

Loading…
Cancel
Save