aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RC_Channel: fixed function indexes above 64

Browse Source
This commit is contained in:
Andrew Tridgell 2016-07-24 12:09:12 +10:00
parent 8e959d4704
commit 7ea68f8f80
2 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/RC_Channel/RC_Channel_aux.cpp
View File

@ -20,9 +20,23 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] = {
}; };
RC_Channel_aux *RC_Channel_aux::_aux_channels[RC_AUX_MAX_CHANNELS]; RC_Channel_aux *RC_Channel_aux::_aux_channels[RC_AUX_MAX_CHANNELS];
uint64_t RC_Channel_aux::_function_mask; uint64_t RC_Channel_aux::_function_mask[2];
bool RC_Channel_aux::_initialised; bool RC_Channel_aux::_initialised;
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 /// map a function to a servo channel and output it
void void
RC_Channel_aux::output_ch(void) RC_Channel_aux::output_ch(void)
@ -123,7 +137,7 @@ void RC_Channel_aux::aux_servo_function_setup(void)
} }
if (function < k_nr_aux_servo_functions) { if (function < k_nr_aux_servo_functions) {
_function_mask |= (1ULL<<(uint8_t)function); set_function_mask((uint8_t)function.get());
} }
} }
@ -136,7 +150,7 @@ void RC_Channel_aux::aux_servo_function_setup(void)
/// (do not call this twice with different parameters, the second call will reset the effect of the first call) /// (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) void RC_Channel_aux::update_aux_servo_function(void)
{ {
_function_mask = 0; clear_function_mask();
// set auxiliary ranges // set auxiliary ranges
for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) {
@ -381,7 +395,10 @@ bool
RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function) RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t function)
{ {
if (function < k_nr_aux_servo_functions) { if (function < k_nr_aux_servo_functions) {
return (_function_mask & (1ULL<<function)) != 0; 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; return false;
} }

4
libraries/RC_Channel/RC_Channel_aux.h
View File

@ -163,9 +163,11 @@ public:
static bool find_channel(Aux_servo_function_t function, uint8_t &chan); static bool find_channel(Aux_servo_function_t function, uint8_t &chan);
private: private:
static uint64_t _function_mask; static uint64_t _function_mask[2];
static bool _initialised; static bool _initialised;
static RC_Channel_aux *_aux_channels[RC_AUX_MAX_CHANNELS]; static RC_Channel_aux *_aux_channels[RC_AUX_MAX_CHANNELS];
void aux_servo_function_setup(void); void aux_servo_function_setup(void);
static void set_function_mask(uint8_t function);
static void clear_function_mask(void);
}; };