|
|
|
@ -2,12 +2,12 @@
@@ -2,12 +2,12 @@
|
|
|
|
|
#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) |
|
|
|
|
# include <stdio.h> |
|
|
|
|
# define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while (0) |
|
|
|
|
#else |
|
|
|
|
# define Debug(fmt, args ...) |
|
|
|
|
#endif |
|
|
|
@ -18,7 +18,7 @@ void RCOutput::init() {}
@@ -18,7 +18,7 @@ void RCOutput::init() {}
|
|
|
|
|
|
|
|
|
|
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) |
|
|
|
|
{ |
|
|
|
|
Debug("set_freq(0x%04x, %u)\n", (unsigned)chmask, (unsigned)freq_hz); |
|
|
|
|
Debug("set_freq(0x%04x, %u)\n", static_cast<uint32_t>(chmask), static_cast<uint32_t>(freq_hz)); |
|
|
|
|
_freq_hz = freq_hz; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -29,18 +29,18 @@ uint16_t RCOutput::get_freq(uint8_t ch)
@@ -29,18 +29,18 @@ uint16_t RCOutput::get_freq(uint8_t ch)
|
|
|
|
|
|
|
|
|
|
void RCOutput::enable_ch(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
if (!(_enable_mask & (1U<<ch))) { |
|
|
|
|
if (!(_enable_mask & (1U << ch))) { |
|
|
|
|
Debug("enable_ch(%u)\n", ch); |
|
|
|
|
} |
|
|
|
|
_enable_mask |= 1U<<ch; |
|
|
|
|
_enable_mask |= 1U << ch; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RCOutput::disable_ch(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
if (_enable_mask & (1U<<ch)) { |
|
|
|
|
if (_enable_mask & (1U << ch)) { |
|
|
|
|
Debug("disable_ch(%u)\n", ch); |
|
|
|
|
} |
|
|
|
|
_enable_mask &= ~1U<<ch; |
|
|
|
|
_enable_mask &= ~1U << ch; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RCOutput::write(uint8_t ch, uint16_t period_us) |
|
|
|
@ -64,18 +64,18 @@ uint16_t RCOutput::read(uint8_t ch)
@@ -64,18 +64,18 @@ uint16_t RCOutput::read(uint8_t ch)
|
|
|
|
|
|
|
|
|
|
void RCOutput::read(uint16_t* period_us, uint8_t len) |
|
|
|
|
{ |
|
|
|
|
memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t)); |
|
|
|
|
memcpy(period_us, _sitlState->pwm_output, len * sizeof(uint16_t)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RCOutput::cork(void) |
|
|
|
|
{ |
|
|
|
|
memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS*sizeof(uint16_t)); |
|
|
|
|
memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS * sizeof(uint16_t)); |
|
|
|
|
_corked = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RCOutput::push(void) |
|
|
|
|
{ |
|
|
|
|
memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS*sizeof(uint16_t)); |
|
|
|
|
memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS * sizeof(uint16_t)); |
|
|
|
|
_corked = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|