diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 9ab6b7c620..7e85e110ae 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -75,9 +74,6 @@ void PX4RCOutput::init() for (uint8_t i=0; i < PX4_NUM_OUTPUT_CHANNELS; i++) { _period[i] = PWM_IGNORE_THIS_CHANNEL; } - - // publish actuator vaules on demand - _actuator_direct_pub = nullptr; } @@ -471,7 +467,7 @@ void PX4RCOutput::_send_outputs(void) } if (to_send > 0) { _arm_actuators(true); - + ::write(_pwm_fd, _period, to_send*sizeof(_period[0])); } if (_max_channel > _servo_count) { diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index 732d85a4fc..97e7afbe0a 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -62,13 +62,11 @@ private: } _outputs[ORB_MULTI_MAX_INSTANCES] {}; actuator_armed_s _armed; - orb_advert_t _actuator_direct_pub; orb_advert_t _actuator_armed_pub; uint16_t _esc_pwm_min; uint16_t _esc_pwm_max; void _init_alt_channels(void); - void _publish_actuators(void); void _arm_actuators(bool arm); void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz, uint32_t &rate_mask); bool _corking;