Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  *       RC_Channels.cpp - class containing an array of RC_Channel objects
      18             :  *
      19             :  */
      20             : 
      21             : #include "RC_Channel_config.h"
      22             : 
      23             : #if AP_RC_CHANNEL_ENABLED
      24             : 
      25             : #include <stdlib.h>
      26             : #include <cmath>
      27             : 
      28             : #include <AP_HAL/AP_HAL.h>
      29             : extern const AP_HAL::HAL& hal;
      30             : 
      31             : #include <AP_Math/AP_Math.h>
      32             : #include <AP_Logger/AP_Logger.h>
      33             : #include <AP_RCMapper/AP_RCMapper.h>
      34             : #include <GCS_MAVLink/GCS.h>
      35             : 
      36             : #include "RC_Channel.h"
      37             : 
      38             : #include <AP_Arming/AP_Arming.h>
      39             : 
      40             : /*
      41             :   channels group object constructor
      42             :  */
      43        2094 : RC_Channels::RC_Channels(void) :
      44        2094 :     override_start_throttle(-1)
      45             : {
      46             :     // set defaults from the parameter table
      47        2094 :     AP_Param::setup_object_defaults(this, var_info);
      48             : 
      49        2094 :     if (_singleton != nullptr) {
      50           0 :         AP_HAL::panic("RC_Channels must be singleton");
      51             :     }
      52        2094 :     _singleton = this;
      53        2094 : }
      54             : 
      55        2091 : void RC_Channels::init(void)
      56             : {
      57             :     // setup ch_in on channels
      58       35547 :     for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
      59       33456 :         channel(i)->ch_in = i;
      60             :     }
      61             : 
      62        2091 :     init_aux_all();
      63        2091 : }
      64             : 
      65   136073620 : bool RC_Channels::has_valid_input() const
      66             : {
      67             :     // the vehicles override this method and check many more
      68             :     // things, but also call this method:
      69   136073620 :     if (!has_ever_seen_rc_input()) {
      70      145822 :         return false;
      71             :     }
      72             : 
      73   135927798 :     return true;
      74             : }
      75             : 
      76     3318000 : uint8_t RC_Channels::get_radio_in(uint16_t *chans, const uint8_t num_channels)
      77             : {
      78     3318000 :     memset(chans, 0, num_channels*sizeof(*chans));
      79             : 
      80     3318000 :     const uint8_t read_channels = MIN(num_channels, NUM_RC_CHANNELS);
      81    56405872 :     for (uint8_t i = 0; i < read_channels; i++) {
      82    53087872 :         chans[i] = channel(i)->get_radio_in();
      83             :     }
      84             : 
      85     3318000 :     return read_channels;
      86             : }
      87             : 
      88             : // update all the input channels
      89    36177494 : bool RC_Channels::read_input(void)
      90             : {
      91    43678933 :     if (hal.rcin->new_input() &&
      92     7501439 :         !rc().option_is_enabled(RC_Channels::Option::IGNORE_RECEIVER)) {
      93     7497042 :         _has_had_rc_receiver = true;
      94    28680452 :     } else if (!has_new_overrides) {
      95    28680077 :         return false;
      96             :     }
      97             : 
      98     7497417 :     _has_ever_seen_rc_input = true;
      99             : 
     100     7497417 :     has_new_overrides = false;
     101             : 
     102     7497417 :     last_update_ms = AP_HAL::millis();
     103             : 
     104     7497417 :     bool success = false;
     105   127456089 :     for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
     106   119958672 :         success |= channel(i)->update();
     107             :     }
     108             : 
     109     7497417 :     if (success) {
     110     7497417 :         rudder_arm_disarm_check();
     111             : 
     112             :         // check if RC overrides should be ignored based on RC_OPTIONS and any pilot input change during active overrides
     113     7497417 :         if (should_ignore_overrides()) {
     114           6 :             set_gcs_overrides_enabled(false);
     115           6 :             GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RC overrides cleared by pilot input");
     116             :         }
     117             :     }
     118             : 
     119     7497417 :     return success;
     120             : }
     121             : 
     122     7497417 : bool RC_Channels::should_ignore_overrides(void)
     123             : {
     124     7497417 :     if (!rc().option_is_enabled(Option::CLEAR_OVERRIDES_BY_RC) || !has_active_overrides()) {
     125     7496865 :         return false;
     126             :     }
     127         552 :     return has_pilot_input_for_override_clear();
     128             : }
     129             : 
     130        1041 : bool RC_Channels::channel_outside_trim_dz(const RC_Channel &ch)
     131             : {
     132        1041 :     return !ch.in_raw_trim_dz();
     133             : }
     134             : 
     135         548 : bool RC_Channels::throttle_moved_since_override_start() const
     136             : {
     137         548 :     if (override_start_throttle < 0) {
     138           0 :         return false;
     139             :     }
     140             : 
     141         548 :     const RC_Channel &thr = get_throttle_channel();
     142         548 :     return abs(thr.get_raw_radio_in() - override_start_throttle) > thr.get_dead_zone();
     143             : }
     144             : 
     145         246 : bool RC_Channels::has_pilot_input_for_override_clear()
     146             : {
     147         491 :     if (channel_outside_trim_dz(get_roll_channel()) ||
     148         491 :         channel_outside_trim_dz(get_pitch_channel()) ||
     149         244 :         channel_outside_trim_dz(get_yaw_channel())) {
     150           3 :         return true;
     151             :     }
     152             : 
     153         243 :     if (throttle_moved_since_override_start()) {
     154           1 :         return true;
     155             :     }
     156             : 
     157         242 :     return false;
     158             : }
     159             : 
     160     1311839 : uint8_t RC_Channels::get_valid_channel_count(void)
     161             : {
     162     1311839 :     return MIN(NUM_RC_CHANNELS, hal.rcin->num_channels());
     163             : }
     164             : 
     165           0 : int16_t RC_Channels::get_receiver_rssi(void)
     166             : {
     167           0 :     return hal.rcin->get_rssi();
     168             : }
     169           0 : int16_t RC_Channels::get_receiver_link_quality(void)
     170             : {
     171           0 :     return hal.rcin->get_rx_link_quality();
     172             : }
     173       60489 : void RC_Channels::clear_overrides(void)
     174             : {
     175       60489 :     RC_Channels &_rc = rc();
     176     1028313 :     for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
     177      967824 :         _rc.channel(i)->clear_override();
     178             :     }
     179             :     // we really should set has_new_overrides to true, and rerun read_input from
     180             :     // the vehicle code however doing so currently breaks the failsafe system on
     181             :     // copter and plane, RC_Channels needs to control failsafes to resolve this
     182       60489 : }
     183             : 
     184     2200594 : uint16_t RC_Channels::get_override_mask(void) const
     185             : {
     186     2200594 :     uint16_t ret = 0;
     187     2200594 :     RC_Channels &_rc = rc();
     188    37410098 :     for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
     189    35209504 :         if (_rc.channel(i)->has_override()) {
     190       15155 :             ret |= (1U << i);
     191             :         }
     192             :     }
     193     2200594 :     return ret;
     194             : }
     195             : 
     196        2107 : void RC_Channels::set_override(const uint8_t chan, const int16_t value, const uint32_t timestamp_ms)
     197             : {
     198        2107 :     RC_Channels &_rc = rc();
     199        2107 :     if (chan < NUM_RC_CHANNELS) {
     200        2107 :         _rc.channel(chan)->set_override(value, timestamp_ms);
     201             :     }
     202        2107 : }
     203             : 
     204      974274 : bool RC_Channels::has_active_overrides()
     205             : {
     206      974274 :     RC_Channels &_rc = rc();
     207    16501586 :     for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) {
     208    15531341 :         if (_rc.channel(i)->has_override()) {
     209        4029 :             return true;
     210             :         }
     211             :     }
     212             : 
     213      970245 :     return false;
     214             : }
     215             : 
     216             : 
     217             : // support for auxiliary switches:
     218             : // read_aux_switches - checks aux switch positions and invokes configured actions
     219     1486823 : void RC_Channels::read_aux_all()
     220             : {
     221     1486823 :     if (!has_valid_input()) {
     222             :         // exit immediately when no RC input
     223       11400 :         return;
     224             :     }
     225     1475423 :     bool need_log = false;
     226             : 
     227    25082191 :     for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
     228    23606768 :         RC_Channel *c = channel(i);
     229    23606768 :         if (c == nullptr) {
     230           0 :             continue;
     231             :         }
     232    23606768 :         need_log |= c->read_aux();
     233             :     }
     234             : #if HAL_LOGGING_ENABLED
     235     1475423 :     if (need_log) {
     236             :         // guarantee that we log when a switch changes
     237        1771 :         AP::logger().Write_RCIN();
     238             :     }
     239             : #endif
     240             : }
     241             : 
     242        2091 : void RC_Channels::init_aux_all()
     243             : {
     244       35547 :     for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
     245       33456 :         RC_Channel *c = channel(i);
     246       33456 :         if (c == nullptr) {
     247           0 :             continue;
     248             :         }
     249       33456 :         c->init_aux();
     250             :     }
     251        2091 :     reset_mode_switch();
     252        2091 : }
     253             : 
     254             : //
     255             : // Support for mode switches
     256             : //
     257    32644586 : RC_Channel *RC_Channels::flight_mode_channel()
     258             : {
     259    32644586 :     const int8_t num = flight_mode_channel_number();
     260    32644586 :     if (num <= 0) {
     261     2222946 :         return nullptr;
     262             :     }
     263    30421640 :     if (num >= NUM_RC_CHANNELS) {
     264           0 :         return nullptr;
     265             :     }
     266    30421640 :     return channel(num-1);
     267             : }
     268        1028 : const RC_Channel *RC_Channels::flight_mode_channel() const
     269             : {
     270        1028 :     const int8_t num = flight_mode_channel_number();
     271        1028 :     if (num <= 0) {
     272             :         // avoid integer underflow on e.g. -1
     273          47 :         return nullptr;
     274             :     }
     275         981 :     return channel(num-1);
     276             : }
     277             : 
     278        2791 : void RC_Channels::reset_mode_switch()
     279             : {
     280        2791 :     RC_Channel *c = flight_mode_channel();
     281        2791 :     if (c == nullptr) {
     282          84 :         return;
     283             :     }
     284        2707 :     c->reset_mode_switch();
     285             : }
     286             : 
     287    33073210 : void RC_Channels::read_mode_switch()
     288             : {
     289    33073210 :     if (!has_valid_input()) {
     290             :         // exit immediately when no RC input
     291      431415 :         return;
     292             :     }
     293    32641795 :     RC_Channel *c = flight_mode_channel();
     294    32641795 :     if (c == nullptr) {
     295     2222862 :         return;
     296             :     }
     297    30418933 :     c->read_mode_switch();
     298             : }
     299             : 
     300             : // check if flight mode channel is assigned RC option
     301             : // return true if assigned
     302        1028 : bool RC_Channels::flight_mode_channel_conflicts_with_rc_option() const
     303             : {
     304        1028 :     const RC_Channel *chan = flight_mode_channel();
     305        1028 :     if (chan == nullptr) {
     306          47 :         return false;
     307             :     }
     308         981 :     return (RC_Channel::AUX_FUNC)chan->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING;
     309             : }
     310             : 
     311             : /*
     312             :   get the RC input PWM value given a channel number.  Note that
     313             :   channel numbers start at 1, as this API is designed for use in
     314             :   LUA
     315             : */
     316         121 : bool RC_Channels::get_pwm(uint8_t c, uint16_t &pwm) const
     317             : {
     318         121 :     const RC_Channel *chan = channel(c-1);
     319         121 :     if (chan == nullptr) {
     320           0 :         return false;
     321             :     }
     322         121 :     int16_t pwm_signed = chan->get_radio_in();
     323         121 :     if (pwm_signed < 0) {
     324           0 :         return false;
     325             :     }
     326         121 :     pwm = (uint16_t)pwm_signed;
     327         121 :     return true;
     328             : }
     329             : 
     330             : // return mask of enabled protocols.
     331     1043641 : uint32_t RC_Channels::enabled_protocols() const
     332             : {
     333     1043641 :     if (_singleton == nullptr) {
     334             :         // for example firmware
     335           0 :         return 1U;
     336             :     }
     337     1043641 :     return uint32_t(_protocols.get());
     338             : }
     339             : 
     340             : #if AP_SCRIPTING_ENABLED
     341             : /*
     342             :   implement aux function cache for scripting
     343             :  */
     344             : 
     345             : /*
     346             :   get last aux cached value for scripting. Returns false if never set, otherwise 0,1,2
     347             : */
     348       25137 : bool RC_Channels::get_aux_cached(RC_Channel::AUX_FUNC aux_fn, uint8_t &pos)
     349             : {
     350       25137 :     const uint16_t aux_idx = uint16_t(aux_fn);
     351       25137 :     if (aux_idx >= unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)) {
     352           0 :         return false;
     353             :     }
     354       25137 :     WITH_SEMAPHORE(aux_cache_sem);
     355       25137 :     uint8_t v = aux_cached.get(aux_idx*2) | (aux_cached.get(aux_idx*2+1)<<1);
     356       25137 :     if (v == 0) {
     357             :         // never been set
     358        1638 :         return false;
     359             :     }
     360       23499 :     pos = v-1;
     361       23499 :     return true;
     362       25137 : }
     363             : 
     364             : /*
     365             :   set cached value of an aux function
     366             :  */
     367        2139 : void RC_Channels::set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwitchPos pos)
     368             : {
     369        2139 :     const uint16_t aux_idx = uint16_t(aux_fn);
     370        2139 :     if (aux_idx < unsigned(RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX)) {
     371        2137 :         WITH_SEMAPHORE(aux_cache_sem);
     372        2137 :         uint8_t v = unsigned(pos)+1;
     373        2137 :         aux_cached.setonoff(aux_idx*2, v&1);
     374        2137 :         aux_cached.setonoff(aux_idx*2+1, v>>1);
     375        2137 :     }
     376        2139 : }
     377             : #endif // AP_SCRIPTING_ENABLED
     378             : 
     379             : #if AP_RCMAPPER_ENABLED
     380             : // these methods return an RC_Channel pointers based on values from
     381             : // AP_::rcmap().  The return value is guaranteed to be not-null to
     382             : // allow use of the pointer without checking it for null-ness.  If an
     383             : // invalid option has been chosen somehow then the returned channel
     384             : // will be a dummy channel.
     385             : static RC_Channel dummy_rcchannel;
     386         548 : const RC_Channel &RC_Channels::get_rcmap_channel_nonnull(uint8_t rcmap_number) const
     387             : {
     388         548 :     const RC_Channel *ret = channel(rcmap_number-1);
     389         548 :     if (ret != nullptr) {
     390         548 :         return *ret;
     391             :     }
     392           0 :     return dummy_rcchannel;
     393             : }
     394      313423 : RC_Channel &RC_Channels::get_rcmap_channel_nonnull(uint8_t rcmap_number)
     395             : {
     396      313423 :     RC_Channel *ret = channel(rcmap_number-1);
     397      313423 :     if (ret != nullptr) {
     398      313422 :         return *ret;
     399             :     }
     400           1 :     return dummy_rcchannel;
     401             : }
     402           0 : const RC_Channel &RC_Channels::get_roll_channel() const
     403             : {
     404           0 :     return get_rcmap_channel_nonnull(AP::rcmap()->roll());
     405             : };
     406       77247 : RC_Channel &RC_Channels::get_roll_channel()
     407             : {
     408       77247 :     return get_rcmap_channel_nonnull(AP::rcmap()->roll());
     409             : };
     410           0 : const RC_Channel &RC_Channels::get_pitch_channel() const
     411             : {
     412           0 :     return get_rcmap_channel_nonnull(AP::rcmap()->pitch());
     413             : };
     414       64129 : RC_Channel &RC_Channels::get_pitch_channel()
     415             : {
     416       64129 :     return get_rcmap_channel_nonnull(AP::rcmap()->pitch());
     417             : };
     418         548 : const RC_Channel &RC_Channels::get_throttle_channel() const
     419             : {
     420         548 :     return get_rcmap_channel_nonnull(AP::rcmap()->throttle());
     421             : };
     422       94942 : RC_Channel &RC_Channels::get_throttle_channel()
     423             : {
     424       94942 :     return get_rcmap_channel_nonnull(AP::rcmap()->throttle());
     425             : };
     426           0 : const RC_Channel &RC_Channels::get_yaw_channel() const
     427             : {
     428           0 :     return get_rcmap_channel_nonnull(AP::rcmap()->yaw());
     429             : };
     430       76939 : RC_Channel &RC_Channels::get_yaw_channel()
     431             : {
     432       76939 :     return get_rcmap_channel_nonnull(AP::rcmap()->yaw());
     433             : };
     434           0 : const RC_Channel &RC_Channels::get_forward_channel() const
     435             : {
     436           0 :     return get_rcmap_channel_nonnull(AP::rcmap()->forward());
     437             : };
     438          83 : RC_Channel &RC_Channels::get_forward_channel()
     439             : {
     440          83 :     return get_rcmap_channel_nonnull(AP::rcmap()->forward());
     441             : };
     442           0 : const RC_Channel &RC_Channels::get_lateral_channel() const
     443             : {
     444           0 :     return get_rcmap_channel_nonnull(AP::rcmap()->lateral());
     445             : };
     446          83 : RC_Channel &RC_Channels::get_lateral_channel()
     447             : {
     448          83 :     return get_rcmap_channel_nonnull(AP::rcmap()->lateral());
     449             : };
     450             : #endif  // AP_RCMAPPER_ENABLED
     451             : 
     452             : 
     453             : /*
     454             :   check for pilot input on rudder stick for arming/disarming
     455             : */
     456     7497417 : void RC_Channels::rudder_arm_disarm_check()
     457             : {
     458             :     // run no more code if arm/disarm via rudder input channel is
     459             :     // completely disabled.  Further checks using this parameter are
     460             :     // done below.
     461     7497417 :     if (AP::arming().get_rudder_arming_type() == AP_Arming::RudderArming::IS_DISABLED) {
     462       21943 :         return;
     463             :     }
     464             : 
     465     7475474 :     const RC_Channel *channel = get_arming_channel();
     466     7475474 :     if (channel == nullptr) {
     467      356241 :         return;
     468             :     }
     469             : 
     470     7119233 :     const auto control_in = channel->get_control_in();
     471     7119233 :     const auto abs_control_in = abs(control_in);
     472             : 
     473     7119233 :     if (abs_control_in == 0) {
     474     7016233 :         have_seen_neutral_rudder = true;
     475             :     }
     476             : 
     477     7119233 :     if (abs_control_in <= 4000) {
     478             :         // not trying to (or no longer trying to) arm or disarm
     479     7077700 :         rudder_arm_timer = 0;
     480     7077700 :         return;
     481             :     }
     482             : 
     483             :     // enforce correct stick gesture for arming (but not disarming):
     484       41533 :     if (arming_check_throttle() && control_in > 4000) {
     485             :         // only permit arming if the vehicle isn't being commanded to
     486             :         // move via RC input
     487       24895 :         const auto &c = rc().get_throttle_channel();
     488       24895 :         if (c.get_control_in() != 0) {
     489       13122 :             rudder_arm_timer = 0;
     490       13122 :             return;
     491             :         }
     492             :     }
     493             : 
     494       28411 :     const uint32_t now = AP_HAL::millis();
     495       28411 :     if (rudder_arm_timer == 0) {
     496             :         // first time we've seen the attempt
     497         218 :         rudder_arm_timer = now;
     498         218 :         return;
     499             :     }
     500             : 
     501       28193 :     if (now - rudder_arm_timer < 3000) {
     502             :         // not time yet....
     503       28018 :         return;
     504             :     }
     505             : 
     506             :     // time to try to arm or disarm:
     507         175 :     rudder_arm_timer = 0;
     508         175 :     if (control_in > 4000) {
     509          74 :         AP::arming().arm(AP_Arming::Method::RUDDER);
     510          74 :         have_seen_neutral_rudder = false;
     511             :     } else {
     512         101 :         if (AP::arming().get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM) {
     513          59 :             AP::arming().disarm(AP_Arming::Method::RUDDER);
     514             :         }
     515             :     }
     516             : }
     517             : 
     518             : // singleton instance
     519             : RC_Channels *RC_Channels::_singleton;
     520             : 
     521             : 
     522   665565158 : RC_Channels &rc()
     523             : {
     524   665565158 :     return *RC_Channels::get_singleton();
     525             : }
     526             : 
     527             : #endif  // AP_RC_CHANNEL_ENABLED