diff --git a/libraries/AP_HAL_PX4/RCOutput_Tap.cpp b/libraries/AP_HAL_PX4/RCOutput_Tap.cpp new file mode 100644 index 0000000000..4557337473 --- /dev/null +++ b/libraries/AP_HAL_PX4/RCOutput_Tap.cpp @@ -0,0 +1,103 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_AEROFC_V1 + +#include "RCOutput_Tap.h" + +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +using namespace PX4; + +void RCOutput_Tap::init() +{ + _perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout"); +} + +/* + set output frequency + */ +void RCOutput_Tap::set_freq(uint32_t chmask, uint16_t freq_hz) +{ +} + +uint16_t RCOutput_Tap::get_freq(uint8_t ch) +{ + return 400; +} + +void RCOutput_Tap::enable_ch(uint8_t ch) +{ + if (ch >= MAX_MOTORS) { + return; + } + _enabled_channels |= (1U << ch); + if (_period[ch] == UINT16_MAX) { + _period[ch] = 0; + } +} + +void RCOutput_Tap::disable_ch(uint8_t ch) +{ + if (ch >= MAX_MOTORS) { + return; + } + + _enabled_channels &= ~(1U << ch); + _period[ch] = UINT16_MAX; +} + +void RCOutput_Tap::write(uint8_t ch, uint16_t period_us) +{ + if (ch >= MAX_MOTORS) { + return; + } + if (!(_enabled_channels & (1U << ch))) { + // not enabled + return; + } + + _period[ch] = period_us; +} + +uint16_t RCOutput_Tap::read(uint8_t ch) +{ + if (ch >= MAX_MOTORS) { + return 0; + } + return _period[ch]; +} + +void RCOutput_Tap::read(uint16_t *period_us, uint8_t len) +{ + for (uint8_t i = 0; i < len; i++) { + period_us[i] = read(i); + } +} + +void RCOutput_Tap::_send_outputs(void) +{ + uint32_t __attribute__((unused)) now = AP_HAL::micros(); + + perf_begin(_perf_rcout); + + perf_end(_perf_rcout); +} + +void RCOutput_Tap::cork() +{ + _corking = true; +} + +void RCOutput_Tap::push() +{ + _corking = false; + _send_outputs(); +} + +#endif diff --git a/libraries/AP_HAL_PX4/RCOutput_Tap.h b/libraries/AP_HAL_PX4/RCOutput_Tap.h new file mode 100644 index 0000000000..b96e203398 --- /dev/null +++ b/libraries/AP_HAL_PX4/RCOutput_Tap.h @@ -0,0 +1,43 @@ +#pragma once + +#include "AP_HAL_PX4.h" +#include +#include +#include + +namespace PX4 { + +class RCOutput_Tap : public AP_HAL::RCOutput { +public: + 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; + + void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override { + _esc_pwm_min = min_pwm; + _esc_pwm_max = max_pwm; + } + + void cork() override; + void push() override; + +private: + static const uint8_t MAX_MOTORS = 4; + + void _send_outputs(void); + + uint8_t _enabled_channels = 0; + bool _corking = false; + + uint16_t _period[MAX_MOTORS]; + uint16_t _esc_pwm_min = 0; + uint16_t _esc_pwm_max = 0; + perf_counter_t _perf_rcout; +}; + +}