From 238e9120003cf7b543cab3246ec61289f8523267 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 4 Jan 2016 17:48:14 +1100
Subject: [PATCH] RC_Channel: allow find_channel() to be called early

and fix a bug!
---
 libraries/RC_Channel/RC_Channel_aux.cpp | 7 ++++++-
 libraries/RC_Channel/RC_Channel_aux.h   | 1 +
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp
index beaa250b8a..20847d88b9 100644
--- a/libraries/RC_Channel/RC_Channel_aux.cpp
+++ b/libraries/RC_Channel/RC_Channel_aux.cpp
@@ -21,6 +21,7 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] = {
 
 RC_Channel_aux *RC_Channel_aux::_aux_channels[RC_AUX_MAX_CHANNELS];
 uint64_t RC_Channel_aux::_function_mask;
+bool RC_Channel_aux::_initialised;
 
 /// map a function to a servo channel and output it
 void
@@ -132,6 +133,7 @@ void RC_Channel_aux::update_aux_servo_function(void)
         if (_aux_channels[i] == NULL) continue;
         _aux_channels[i]->aux_servo_function_setup();
 	}
+    _initialised = true;
 }
 
 
@@ -404,11 +406,14 @@ bool RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::Aux_servo_function_
 // 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) {
+        if (_aux_channels[i] && _aux_channels[i]->function == function) {
             chan = _aux_channels[i]->_ch_out;
             return true;
         }
diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h
index 0e868d1d96..d986d57788 100644
--- a/libraries/RC_Channel/RC_Channel_aux.h
+++ b/libraries/RC_Channel/RC_Channel_aux.h
@@ -146,6 +146,7 @@ public:
     
 private:
     static uint64_t _function_mask;
+    static bool _initialised;
     static RC_Channel_aux *_aux_channels[RC_AUX_MAX_CHANNELS];
 
     void aux_servo_function_setup(void);