libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp

   1 #include "AP_RCProtocol_config.h"
   2
   3 #if AP_RCPROTOCOL_FDM_ENABLED
   4
   5 #include "AP_RCProtocol_FDM.h"
   6
   7 #include <AP_HAL/AP_HAL.h>
   8 #include <SITL/SITL.h>
   9
  10 extern const AP_HAL::HAL& hal;
  11
  12 void AP_RCProtocol_FDM::update()
  13 {
  14     const auto sitl = AP::sitl();
  15     if (sitl == nullptr) {
  16         return;
  17     }
  18
  19     const auto &fdm = sitl->state;
  20
  21     // see if there's fresh input.  Until timestamps are worked out,
  22     // just check for non-zero values:
  23     if (fdm.rcin_chan_count == 0) {
  24         return;
  25     }
  26
  27     // simulate RC input at 50Hz
  28     if (AP_HAL::millis() - last_input_ms < 20) {
  29         return;
  30     }
  31
  32     last_input_ms = AP_HAL::millis();
  33
  34     // scale from FDM 0-1 floats to PWM values
  35     // these are the values that will be fed into the autopilot.
  36     uint16_t pwm_input[16];
  37     const uint8_t count = MIN(ARRAY_SIZE(pwm_input), fdm.rcin_chan_count);
  38     for (uint8_t i=0; i<count; i++) {
  39         pwm_input[i] = 1000 + fdm.rcin[i] * 1000;
  40     }
  41     add_input(
  42         count,
  43         pwm_input,
  44         false,  // failsafe
  45         0, // check me
  46         0  // link quality
  47         );
  48 }
  49
  50 #endif // AP_RCPROTOCOL_FDM_ENABLED