From 25bf1abecffeb0b4c29386ef6a019b7a60c23899 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 31 Oct 2013 10:29:06 +0100 Subject: [PATCH] pwm_output: Allow PWM values from 900us to 2100us but use a default of 1000us to 2000us --- src/drivers/drv_pwm_output.h | 18 +++++++++++++---- src/drivers/px4fmu/fmu.cpp | 28 +++++++++++++-------------- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/registers.c | 28 +++++++++++++-------------- src/systemcmds/esc_calib/esc_calib.c | 10 +++++----- 5 files changed, 48 insertions(+), 38 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index efd6afb4bd..51f916f37b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,9 +65,14 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** - * Minimum PWM in us + * Lowest minimum PWM in us */ -#define PWM_MIN 900 +#define PWM_LOWEST_MIN 900 + +/** + * Default minimum PWM in us + */ +#define PWM_DEFAULT_MIN 1000 /** * Highest PWM allowed as the minimum PWM @@ -75,9 +80,14 @@ __BEGIN_DECLS #define PWM_HIGHEST_MIN 1300 /** - * Maximum PWM in us + * Highest maximum PWM in us */ -#define PWM_MAX 2100 +#define PWM_HIGHEST_MAX 2100 + +/** + * Default maximum PWM in us + */ +#define PWM_DEFAULT_MAX 2000 /** * Lowest PWM allowed as the maximum PWM diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4bd27f9076..0441566e98 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -232,8 +232,8 @@ PX4FMU::PX4FMU() : _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { - _min_pwm[i] = PWM_MIN; - _max_pwm[i] = PWM_MAX; + _min_pwm[i] = PWM_DEFAULT_MIN; + _max_pwm[i] = PWM_DEFAULT_MAX; } _debug_enabled = true; @@ -762,10 +762,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _failsafe_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _failsafe_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; } else { _failsafe_pwm[i] = pwm->values[i]; } @@ -801,10 +801,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_MAX) { - _disarmed_pwm[i] = PWM_MAX; - } else if (pwm->values[i] < PWM_MIN) { - _disarmed_pwm[i] = PWM_MIN; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; } else { _disarmed_pwm[i] = pwm->values[i]; } @@ -842,8 +842,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { _min_pwm[i] = PWM_HIGHEST_MIN; - } else if (pwm->values[i] < PWM_MIN) { - _min_pwm[i] = PWM_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; } else { _min_pwm[i] = pwm->values[i]; } @@ -872,8 +872,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_MAX) { - _max_pwm[i] = PWM_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; } else { _max_pwm[i] = pwm->values[i]; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 63e775857c..08e697b9f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1985,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { + if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) { ret = -EINVAL; } else { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 40597adf1f..86a40bc228 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; /** * PAGE 107 @@ -215,7 +215,7 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_ * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; /** * PAGE 108 @@ -278,10 +278,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values < PWM_MIN) { - r_page_servo_failsafe[offset] = PWM_MIN; - } else if (*values > PWM_MAX) { - r_page_servo_failsafe[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; } else { r_page_servo_failsafe[offset] = *values; } @@ -304,8 +304,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; - } else if (*values < PWM_MIN) { - r_page_servo_control_min[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; } else { r_page_servo_control_min[offset] = *values; } @@ -323,8 +323,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* ignore 0 */ - } else if (*values > PWM_MAX) { - r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; } else { @@ -348,11 +348,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index c402061971..b237b31be3 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -82,7 +82,7 @@ usage(const char *reason) " [-c ] Supply channels (e.g. 1234)\n" " [-m ] Directly supply channel mask (e.g. 0xF)\n" " [-a] Use all outputs\n" - , PWM_MIN, PWM_MAX); + , PWM_DEFAULT_MIN, PWM_DEFAULT_MAX); } int @@ -100,8 +100,8 @@ esc_calib_main(int argc, char *argv[]) unsigned long channels; unsigned single_ch = 0; - uint16_t pwm_high = PWM_MAX; - uint16_t pwm_low = PWM_MIN; + uint16_t pwm_high = PWM_DEFAULT_MAX; + uint16_t pwm_low = PWM_DEFAULT_MIN; struct pollfd fds; fds.fd = 0; /* stdin */ @@ -148,13 +148,13 @@ esc_calib_main(int argc, char *argv[]) case 'l': /* Read in custom low value */ - if (*ep != '\0' || pwm_low < PWM_MIN || pwm_low > PWM_HIGHEST_MIN) + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) usage("low PWM invalid"); break; case 'h': /* Read in custom high value */ pwm_high = strtoul(optarg, &ep, 0); - if (*ep != '\0' || pwm_high > PWM_MAX || pwm_high < PWM_LOWEST_MAX) + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) usage("high PWM invalid"); break; default: