libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp

   1 #include "AP_RCProtocol_config.h"
   2
   3 #if AP_RCPROTOCOL_UDP_ENABLED
   4
   5 #include "AP_RCProtocol_UDP.h"
   6
   7 #include <AP_HAL/AP_HAL.h>
   8 #include <AP_Vehicle/AP_Vehicle_Type.h>
   9 #include <SITL/SITL.h>
  10
  11 #if AP_RCPROTOCOL_FDM_ENABLED
  12 #include "AP_RCProtocol_FDM.h"
  13 #endif
  14
  15 extern const AP_HAL::HAL& hal;
  16
  17 void AP_RCProtocol_UDP::set_default_pwm_input_values()
  18 {
  19     pwm_input[0] = 1500;
  20     pwm_input[1] = 1500;
  21     pwm_input[2] = 1000;
  22     pwm_input[3] = 1500;
  23     pwm_input[4] = 1800;
  24     pwm_input[5] = 1000;
  25     pwm_input[6] = 1000;
  26     pwm_input[7] = 1800;
  27
  28 #if APM_BUILD_TYPE(APM_BUILD_Rover)
  29     // set correct default throttle for rover (allowing for reverse)
  30     pwm_input[2] = 1500;
  31 #elif APM_BUILD_TYPE(APM_BUILD_ArduSub) || APM_BUILD_TYPE(APM_BUILD_Blimp)
  32     for(uint8_t i = 0; i < 8; i++) {
  33         pwm_input[i] = 1500;
  34     }
  35 #endif
  36
  37     num_channels = 8;
  38 }
  39
  40 bool AP_RCProtocol_UDP::init()
  41 {
  42     const auto sitl = AP::sitl();
  43     if (sitl == nullptr) {
  44         return false;
  45     }
  46     if (!rc_in.reuseaddress()) {
  47         return false;
  48     }
  49     if (!rc_in.bind("0.0.0.0", sitl->rcin_port)) {
  50         return false;
  51     }
  52     if (!rc_in.set_blocking(false)) {
  53         return false;
  54     }
  55     if (!rc_in.set_cloexec()) {
  56         return false;
  57     }
  58
  59     set_default_pwm_input_values();
  60
  61     return true;
  62 }
  63
  64 void AP_RCProtocol_UDP::update()
  65 {
  66 #if AP_RCPROTOCOL_FDM_ENABLED
  67     // yield to the FDM backend if it is getting data
  68     if (fdm_backend->active()) {
  69         return;
  70     }
  71 #endif
  72
  73     if (!init_done) {
  74         if (!init()) {
  75             return;
  76         }
  77         init_done = true;
  78     }
  79
  80     read_all_socket_input();
  81
  82 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
  83     const auto sitl = AP::sitl();
  84     if (sitl == nullptr) {
  85         return;
  86     }
  87
  88     if (sitl->rc_fail == SITL::SIM::SITL_RCFail_NoPulses) {
  89         return;
  90     }
  91 #endif
  92
  93     // simulate RC input at 50Hz
  94     if (AP_HAL::millis() - last_input_ms < 20) {
  95         return;
  96     }
  97     last_input_ms = AP_HAL::millis();
  98
  99     add_input(
 100         num_channels,
 101         pwm_input,
 102         false,  // failsafe
 103         0, // check me
 104         0  // link quality
 105         );
 106 }
 107
 108 /*
 109   check for a SITL RC input packet
 110  */
 111 void AP_RCProtocol_UDP::read_all_socket_input(void)
 112 {
 113     struct pwm_packet {
 114         uint16_t pwm[16];
 115     } pwm_pkt;
 116     uint8_t pwm_pkt_num_channels = 0;
 117
 118     ssize_t receive_size = 1;  // lies!
 119     uint16_t count = 0;
 120     while (receive_size > 0) {
 121         receive_size = rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
 122
 123         switch (receive_size) {
 124         case -1:
 125             break;
 126         case 8*2:
 127         case 16*2:
 128             pwm_pkt_num_channels = receive_size/2;
 129             break;
 130         default:
 131             fprintf(stderr, "Malformed SITL RC input (%ld)", (long)receive_size);
 132             return;
 133         }
 134         count++;
 135     }
 136
 137     if (count > 100) {
 138         ::fprintf(stderr, "Read %u rc inputs\n", count);
 139     }
 140
 141 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 142     const auto sitl = AP::sitl();
 143     if (sitl == nullptr) {
 144         return;
 145     }
 146
 147     // convert last packet received into pwm values
 148     switch (sitl->rc_fail) {
 149     case SITL::SIM::SITL_RCFail_Throttle950:
 150         // discard anything we just read from the "receiver" and set
 151         // values to bind values:
 152         for (uint8_t i=0; i<ARRAY_SIZE(pwm_input); i++) {
 153             pwm_input[i] = 1500;  // centre all inputs
 154         }
 155         pwm_input[2] = 950;  // reset throttle (assumed to be on channel 3...)
 156         return;
 157     case SITL::SIM::SITL_RCFail_NoPulses:
 158         // see also code in ::update
 159         return;
 160     case SITL::SIM::SITL_RCFail_None:
 161         break;
 162     }
 163 #endif
 164
 165     if (pwm_pkt_num_channels == 0) {
 166         return;
 167     }
 168     for (uint8_t i=0; i<pwm_pkt_num_channels; i++) {
 169         // setup the pwm input for the RC channel inputs
 170         const uint16_t pwm = pwm_pkt.pwm[i];
 171         if (pwm == 0) {
 172             // 0 means "ignore this value"
 173             continue;
 174         }
 175         pwm_input[i] = pwm;
 176     }
 177     num_channels = pwm_pkt_num_channels;  // or ARRAY_SIZE(pwm_input)?
 178 }
 179
 180 #endif // AP_RCPROTOCOL_UDP_ENABLED