From 8131d28a0faf7d33060cf067f5bd8dee41666fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Sep 2013 21:35:50 +0200 Subject: [PATCH] Exported disarmed PWM values as IOCTLs --- src/drivers/drv_pwm_output.h | 12 +++- src/drivers/px4io/px4io.cpp | 110 ++++++++++++++++++++--------------- 2 files changed, 75 insertions(+), 47 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca098..6ed9320cb1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -79,6 +79,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; + int channel_count; }; /* @@ -118,9 +119,18 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** Power up DSM receiver */ +/** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +/** set the PWM value when disarmed - should be no PWM (zero) by default */ +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) + +/** set the minimum PWM value the output will send */ +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) + +/** set the maximum PWM value the output will send */ +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a8..bd5f330436 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -177,21 +177,6 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); - /** - * Set the minimum PWM signals when armed - */ - int set_min_values(const uint16_t *vals, unsigned len); - - /** - * Set the maximum PWM signal when armed - */ - int set_max_values(const uint16_t *vals, unsigned len); - - /** - * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE - */ - int set_idle_values(const uint16_t *vals, unsigned len); - /** * Print IO status. * @@ -391,6 +376,21 @@ private: */ int mixer_send(const char *buf, unsigned buflen); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Handle a status update from IO. * @@ -1674,6 +1674,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; } + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_idle_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_min_values(pwm->values, pwm->channel_count); + } + break; + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + set_max_values(pwm->values, pwm->channel_count); + } + break; + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; @@ -2265,26 +2283,26 @@ px4io_main(int argc, char *argv[]) errx(1, "min command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; - /* set values for first 8 channels, fill unassigned channels with 900. */ - uint16_t min[8]; + if (iofd > 0) { - for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) + pwm.channel_count = 0; + + for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) { /* set channel to commanline argument or to 900 for non-provided channels */ if (argc > i + 2) { - min[i] = atoi(argv[i+2]); - if (min[i] < 900 || min[i] > 1200) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 900 || pwm.values[i] > 1200) { errx(1, "value out of range of 900 < value < 1200. Aborting."); } - } else { - /* a zero value will the default */ - min[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting min values"); @@ -2300,26 +2318,26 @@ px4io_main(int argc, char *argv[]) errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; + + if (iofd > 0) { - /* set values for first 8 channels, fill unassigned channels with 2100. */ - uint16_t max[8]; + pwm.channel_count = 0; - for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) + for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) { /* set channel to commanline argument or to 2100 for non-provided channels */ if (argc > i + 2) { - max[i] = atoi(argv[i+2]); - if (max[i] < 1800 || max[i] > 2100) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { errx(1, "value out of range of 1800 < value < 2100. Aborting."); } - } else { - /* a zero value will the default */ - max[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting max values"); @@ -2329,32 +2347,32 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "idle")) { + if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { if (argc < 3) { errx(1, "max command needs at least one channel value (PWM)"); } - if (g_dev != nullptr) { + int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); + struct pwm_output_values pwm; + + if (iofd > 0) { - /* set values for first 8 channels, fill unassigned channels with 0. */ - uint16_t idle[8]; + pwm.channel_count = 0; - for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) { /* set channel to commanline argument or to 0 for non-provided channels */ if (argc > i + 2) { - idle[i] = atoi(argv[i+2]); - if (idle[i] < 900 || idle[i] > 2100) { + pwm.values[i] = atoi(argv[i+2]); + if (pwm.values[i] < 900 || pwm.values[i] > 2100) { errx(1, "value out of range of 900 < value < 2100. Aborting."); } - } else { - /* a zero value will the default */ - idle[i] = 0; + pwm.channel_count++; } } - int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); if (ret != OK) errx(ret, "failed setting idle values");