diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp index 9c6bac9aa7..e02e409cbc 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.cpp +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.cpp @@ -30,39 +30,39 @@ void VRBRAINRCOutput::init(void* unused) hal.console->printf("RCOutput: Unable to setup IO arming OK\n"); } _rate_mask = 0; - _alt_fd = -1; + _servo_count = 0; - _alt_servo_count = 0; + if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) { hal.console->printf("RCOutput: Unable to get servo count\n"); return; } - _alt_fd = open("/dev/vroutput", O_RDWR); - if (_alt_fd == -1) { - hal.console->printf("RCOutput: failed to open /dev/vroutput"); - return; - } + + + + + } void VRBRAINRCOutput::_init_alt_channels(void) { - if (_alt_fd == -1) { - return; - } - if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) { - hal.console->printf("RCOutput: Unable to setup alt IO arming\n"); - return; - } - if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) { - hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n"); - return; - } - if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) { - hal.console->printf("RCOutput: Unable to get servo count\n"); - } + + + + + + + + + + + + + + } void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) @@ -128,12 +128,12 @@ uint16_t VRBRAINRCOutput::get_freq(uint8_t ch) void VRBRAINRCOutput::enable_ch(uint8_t ch) { - if (ch >= 8 && !(_enabled_channels & (1U<= _servo_count + _alt_servo_count) { + if (ch >= _servo_count) { return; } if (!(_enabled_channels & (1U< 0) { - uint8_t n = _max_channel - _servo_count; - if (n > _alt_servo_count) { - n = _alt_servo_count; - } - ::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0])); - } - } + + + + + + + + + + + perf_end(_perf_rcout); _last_output = now; } diff --git a/libraries/AP_HAL_VRBRAIN/RCOutput.h b/libraries/AP_HAL_VRBRAIN/RCOutput.h index 70921fc823..1f71f14642 100644 --- a/libraries/AP_HAL_VRBRAIN/RCOutput.h +++ b/libraries/AP_HAL_VRBRAIN/RCOutput.h @@ -27,7 +27,7 @@ public: private: int _pwm_fd; - int _alt_fd; + uint16_t _freq_hz; uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS]; volatile uint8_t _max_channel; @@ -35,7 +35,7 @@ private: perf_counter_t _perf_rcout; uint32_t _last_output; unsigned _servo_count; - unsigned _alt_servo_count; + uint32_t _rate_mask; uint16_t _enabled_channels;