HAL_SITL: enable debug of rcoutput channel enable and frequency

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -2,24 +2,46 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 #include "RCOutput.h"
+#include <stdio.h>
+
+#define ENABLE_DEBUG 0
+
+#if ENABLE_DEBUG
+# define Debug(fmt, args ...)  do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+# define Debug(fmt, args ...)
+#endif
 
 using namespace HALSITL;
 
 void SITLRCOutput::init() {}
 
-void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
+void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    Debug("set_freq(0x%04x, %u)\n", (unsigned)chmask, (unsigned)freq_hz);
     _freq_hz = freq_hz;
 }
 
-uint16_t SITLRCOutput::get_freq(uint8_t ch) {
+uint16_t SITLRCOutput::get_freq(uint8_t ch)
+{
     return _freq_hz;
 }
 
 void SITLRCOutput::enable_ch(uint8_t ch)
-{}
+{
+    if (!(_enable_mask & (1U<<ch))) {
+        Debug("enable_ch(%u)\n", ch);
+    }
+    _enable_mask |= 1U<<ch;
+}
 
 void SITLRCOutput::disable_ch(uint8_t ch)
-{}
+{
+    if (_enable_mask & (1U<<ch)) {
+        Debug("disable_ch(%u)\n", ch);
+    }
+    _enable_mask &= ~1U<<ch;
+}
 
 void SITLRCOutput::write(uint8_t ch, uint16_t period_us)
 {

libraries/AP_HAL_SITL/RCOutput.h

@@ -12,18 +12,19 @@ public:
         _sitlState = sitlState;
         _freq_hz = 50;
     }
-    void     init();
-    void     set_freq(uint32_t chmask, uint16_t freq_hz);
-    uint16_t get_freq(uint8_t ch);
-    void     enable_ch(uint8_t ch);
-    void     disable_ch(uint8_t ch);
-    void     write(uint8_t ch, uint16_t period_us);
-    uint16_t read(uint8_t ch);
-    void     read(uint16_t* period_us, uint8_t len);
+    void     init() override;
+    void     set_freq(uint32_t chmask, uint16_t freq_hz) override;
+    uint16_t get_freq(uint8_t ch) override;
+    void     enable_ch(uint8_t ch) override;
+    void     disable_ch(uint8_t ch) override;
+    void     write(uint8_t ch, uint16_t period_us) override;
+    uint16_t read(uint8_t ch) override;
+    void     read(uint16_t* period_us, uint8_t len) override;
 
 private:
     SITL_State *_sitlState;
     uint16_t _freq_hz;
+    uint16_t _enable_mask;
 };
 
 #endif