Line data    Source code

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