Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
       4             : 
       5             : #include <AP_BoardConfig/AP_BoardConfig.h>
       6             : #include <SITL/SITL.h>
       7             : 
       8             : #include "RCOutput.h"
       9             : 
      10             : #define ENABLE_DEBUG 0
      11             : 
      12             : #if ENABLE_DEBUG
      13             : # include <stdio.h>
      14             : # define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while (0)
      15             : #else
      16             : # define Debug(fmt, args ...)
      17             : #endif
      18             : 
      19             : using namespace HALSITL;
      20             : 
      21        2094 : void RCOutput::init() {}
      22             : 
      23        2565 : void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
      24             : {
      25             :     Debug("set_freq(0x%04x, %u)\n", static_cast<uint32_t>(chmask), static_cast<uint32_t>(freq_hz));
      26        2565 :     _freq_hz = freq_hz;
      27        2565 : }
      28             : 
      29           0 : uint16_t RCOutput::get_freq(uint8_t ch)
      30             : {
      31           0 :     return _freq_hz;
      32             : }
      33             : 
      34     2420960 : void RCOutput::enable_ch(uint8_t ch)
      35             : {
      36     2420960 :     if (!(_enable_mask & (1U << ch))) {
      37             :         Debug("enable_ch(%u)\n", ch);
      38             :     }
      39     2420960 :     _enable_mask |= (1U << ch);
      40     2420960 : }
      41             : 
      42     2418272 : void RCOutput::disable_ch(uint8_t ch)
      43             : {
      44     2418272 :     if (_enable_mask & (1U << ch)) {
      45             :         Debug("disable_ch(%u)\n", ch);
      46             :     }
      47     2418272 :     _enable_mask &= ~(1U << ch);
      48     2418272 : }
      49             : 
      50   835612130 : void RCOutput::write(uint8_t ch, uint16_t period_us)
      51             : {
      52   835612130 :     if (safety_state == AP_HAL::Util::SAFETY_DISARMED) {
      53     1056253 :         const auto *board_config = AP_BoardConfig::get_singleton();
      54     1056253 :         const uint32_t safety_mask = board_config != nullptr? board_config->get_safety_mask() : 0;
      55     1056253 :         if (!(safety_mask & (1U<<ch))) {
      56             :             // implement safety pwm value
      57     1009420 :             period_us = 0;
      58             :         }
      59             :     }
      60             : 
      61   835612130 :     _sitlState->output_ready = true;
      62             :     // FIXME: something in sitl is expecting to be able to read and write disabled channels
      63   835612130 :     if (ch < SITL_NUM_CHANNELS /*&& (_enable_mask & (1U<<ch))*/) {
      64   835612130 :         if (_corked) {
      65   835465497 :             _pending[ch] = period_us;
      66             :         } else {
      67      146633 :             _sitlState->pwm_output[ch] = period_us;
      68             :         }
      69             :     }
      70   835612130 : }
      71             : 
      72     8806828 : uint16_t RCOutput::read(uint8_t ch)
      73             : {
      74             :     // FIXME: something in sitl is expecting to be able to read and write disabled channels
      75     8806828 :     if (ch < SITL_NUM_CHANNELS /*&& (_enable_mask & (1U<<ch))*/) {
      76     8806828 :         return _sitlState->pwm_output[ch];
      77             :     }
      78           0 :     return 0;
      79             : }
      80             : 
      81     3391063 : void RCOutput::read(uint16_t* period_us, uint8_t len)
      82             : {
      83     3391063 :     memcpy(period_us, _sitlState->pwm_output, len * sizeof(uint16_t));
      84     3391063 : }
      85             : 
      86    50300325 : void RCOutput::cork(void)
      87             : {
      88    50300325 :     if (!_corked) {
      89    41922778 :         memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS * sizeof(uint16_t));
      90    41922778 :         _corked = true;
      91             :     }
      92    50300325 : }
      93             : 
      94    41922778 : void RCOutput::push(void)
      95             : {
      96    41922778 :     if (_corked) {
      97    41922778 :         memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS * sizeof(uint16_t));
      98    41922778 :         _corked = false;
      99             :     }
     100             : 
     101    41922778 :     SITL::SIM *sitl = AP::sitl();
     102    41922778 :     if (sitl && sitl->esc_telem) {
     103    41720409 :         if (esc_telem == nullptr) {
     104        2073 :             esc_telem = NEW_NOTHROW AP_ESC_Telem_SITL;
     105             :         }
     106    41720409 :         if (esc_telem != nullptr) {
     107    41720409 :             esc_telem->update();
     108             :         }
     109             :     }
     110    41922778 : }
     111             : 
     112             : /*
     113             :   Serial LED emulation
     114             : */
     115           0 : bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint32_t clock_mask)
     116             : {
     117           0 :     if (chan > 15 || num_leds > 64) {
     118           0 :         return false;
     119             :     }
     120           0 :     SITL::SIM *sitl = AP::sitl();
     121           0 :     if (sitl) {
     122           0 :         sitl->led.num_leds[chan] = num_leds;
     123           0 :         return true;
     124             :     }
     125           0 :     return false;
     126             : }
     127             : 
     128           0 : bool RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue)
     129             : {
     130           0 :     if (chan > 15) {
     131           0 :         return false;
     132             :     }
     133           0 :     SITL::SIM *sitl = AP::sitl();
     134           0 :     if (led == -1) {
     135           0 :         for (uint8_t i=0; i < sitl->led.num_leds[chan]; i++) {
     136           0 :             set_serial_led_rgb_data(chan, i, red, green, blue);
     137             :         }
     138           0 :         return true;
     139             :     }
     140           0 :     if (led < -1 || led >= sitl->led.num_leds[chan]) {
     141           0 :         return false;
     142             :     }
     143           0 :     if (sitl) {
     144           0 :         sitl->led.rgb[chan][led].rgb[0] = red;
     145           0 :         sitl->led.rgb[chan][led].rgb[1] = green;
     146           0 :         sitl->led.rgb[chan][led].rgb[2] = blue;
     147             :     }
     148           0 :     return true;
     149             : }
     150             : 
     151           0 : bool RCOutput::serial_led_send(const uint16_t chan)
     152             : {
     153           0 :     SITL::SIM *sitl = AP::sitl();
     154           0 :     if (sitl) {
     155           0 :         sitl->led.send_counter++;
     156             :     }
     157           0 :     return true;
     158             : }
     159             : 
     160             : #endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL