From 32994a5b1e113ff6a6d0f7888190f484171e3e6e Mon Sep 17 00:00:00 2001 From: Max Basescu Date: Thu, 4 Jun 2015 19:56:54 -0400 Subject: [PATCH] AP_HAL_AVR: Changed constrain_period to use RC_OUTPUT bounds instead of RC_INPUT Done in order to get full range of motion for servo output --- libraries/AP_HAL_AVR/RCOutput_APM1.cpp | 4 ++-- libraries/AP_HAL_AVR/RCOutput_APM2.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp index ceaa3fe1e4..9d5da0ccc7 100644 --- a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp +++ b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp @@ -148,8 +148,8 @@ void APM1RCOutput::disable_ch(uint8_t ch) { /* constrain pwm to be between min and max pulsewidth. */ static inline uint16_t constrain_period(uint16_t p) { - if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH; - if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH; + if (p > RC_OUTPUT_MAX_PULSEWIDTH) return RC_OUTPUT_MAX_PULSEWIDTH; + if (p < RC_OUTPUT_MIN_PULSEWIDTH) return RC_OUTPUT_MIN_PULSEWIDTH; return p; } diff --git a/libraries/AP_HAL_AVR/RCOutput_APM2.cpp b/libraries/AP_HAL_AVR/RCOutput_APM2.cpp index fdf5106f25..9993f91308 100644 --- a/libraries/AP_HAL_AVR/RCOutput_APM2.cpp +++ b/libraries/AP_HAL_AVR/RCOutput_APM2.cpp @@ -140,8 +140,8 @@ void APM2RCOutput::disable_ch(uint8_t ch) { /* constrain pwm to be between min and max pulsewidth. */ static inline uint16_t constrain_period(uint16_t p) { - if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH; - if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH; + if (p > RC_OUTPUT_MAX_PULSEWIDTH) return RC_OUTPUT_MAX_PULSEWIDTH; + if (p < RC_OUTPUT_MIN_PULSEWIDTH) return RC_OUTPUT_MIN_PULSEWIDTH; return p; }