From 63b3618fc73deb664d70eb9cc08d13c182425da3 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 9 Jan 2017 13:31:44 +0100 Subject: [PATCH] AP_HAL_SITL: RCOuput minor fix correct style make constructor explicit use c++ cast --- libraries/AP_HAL_SITL/RCOutput.cpp | 20 ++++++++++---------- libraries/AP_HAL_SITL/RCOutput.h | 21 +++++++++------------ 2 files changed, 19 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index 3fd9941951..01d8e8994e 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -2,12 +2,12 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "RCOutput.h" -#include #define ENABLE_DEBUG 0 #if ENABLE_DEBUG -# define Debug(fmt, args ...) do {::printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +# include +# 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() {} 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(chmask), static_cast(freq_hz)); _freq_hz = freq_hz; } @@ -29,18 +29,18 @@ uint16_t RCOutput::get_freq(uint8_t ch) void RCOutput::enable_ch(uint8_t ch) { - if (!(_enable_mask & (1U<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; } diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h index c777de06d8..ec72c3d9a0 100644 --- a/libraries/AP_HAL_SITL/RCOutput.h +++ b/libraries/AP_HAL_SITL/RCOutput.h @@ -6,20 +6,17 @@ class HALSITL::RCOutput : public AP_HAL::RCOutput { public: - RCOutput(SITL_State *sitlState) { - _sitlState = sitlState; - _freq_hz = 50; - } - void init() override; - void set_freq(uint32_t chmask, uint16_t freq_hz) override; + explicit RCOutput(SITL_State *sitlState): _sitlState(sitlState), _freq_hz(50) {} + 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; + 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; - void cork(void); - void push(void); + void read(uint16_t* period_us, uint8_t len) override; + void cork(void); + void push(void); private: SITL_State *_sitlState;