Line data    Source code

       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    36177494 : void AP_RCProtocol_FDM::update()
      13             : {
      14    36177494 :     const auto sitl = AP::sitl();
      15    36177494 :     if (sitl == nullptr) {
      16    36177494 :         return;
      17             :     }
      18             : 
      19    36177494 :     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    36177494 :     if (fdm.rcin_chan_count == 0) {
      24    36177494 :         return;
      25             :     }
      26             : 
      27             :     // simulate RC input at 50Hz
      28           0 :     if (AP_HAL::millis() - last_input_ms < 20) {
      29           0 :         return;
      30             :     }
      31             : 
      32           0 :     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           0 :     const uint8_t count = MIN(ARRAY_SIZE(pwm_input), fdm.rcin_chan_count);
      38           0 :     for (uint8_t i=0; i<count; i++) {
      39           0 :         pwm_input[i] = 1000 + fdm.rcin[i] * 1000;
      40             :     }
      41           0 :     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