diff options
author | px4dev <px4@purgatory.org> | 2013-03-12 00:40:22 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-03-12 22:22:50 -0700 |
commit | 7011fe563b583d7940247a7a01265b7f8675fee8 (patch) | |
tree | a497bcf8ba9eb4ad20f70185efe1d9df88f7ba99 | |
parent | 57429fd12cc4277c88948c1819b245d9e83523d2 (diff) | |
download | px4-firmware-7011fe563b583d7940247a7a01265b7f8675fee8.tar.gz px4-firmware-7011fe563b583d7940247a7a01265b7f8675fee8.tar.bz2 px4-firmware-7011fe563b583d7940247a7a01265b7f8675fee8.zip |
Move PWM rate configuration, etc. into a separate utility and out of the individual drivers.
-rw-r--r-- | apps/drivers/hil/hil.cpp | 25 | ||||
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 56 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 50 | ||||
-rw-r--r-- | apps/systemcmds/pwm/Makefile | 42 | ||||
-rw-r--r-- | apps/systemcmds/pwm/pwm.c | 207 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 1 |
6 files changed, 289 insertions, 92 deletions
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 8a471b61c..d9aa772d4 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -655,7 +655,7 @@ enum PortMode { PortMode g_port_mode; int -hil_new_mode(PortMode new_mode, int update_rate) +hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; @@ -713,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_hil->set_mode(servo_mode); - if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0)) - g_hil->set_pwm_rate(update_rate); return OK; } @@ -800,7 +798,6 @@ int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - unsigned pwm_rate = 0; const char *verb = argv[1]; if (hil_start() != OK) @@ -812,22 +809,6 @@ hil_main(int argc, char *argv[]) // this was all cut-and-pasted from the FMU driver; it's junk if (!strcmp(verb, "mode_pwm")) { - int ch; - - while ((ch = getopt(argc - 1, argv + 1, "u:")) != EOF) { - switch (ch) { - case 'u': - pwm_rate = strtol(optarg, nullptr, 0); - break; - - case ':': - errx(1, "missing parameter"); - - default: - errx(1, "unrecognised option"); - } - } - new_mode = PORT1_FULL_PWM; } else if (!strcmp(verb, "mode_pwm_serial")) { @@ -854,7 +835,7 @@ hil_main(int argc, char *argv[]) return OK; /* switch modes */ - return hil_new_mode(new_mode, pwm_rate); + return hil_new_mode(new_mode); } if (!strcmp(verb, "test")) @@ -865,6 +846,6 @@ hil_main(int argc, char *argv[]) fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); return -EINVAL; } diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 7f2c7a19b..476adb7f0 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -309,6 +309,8 @@ PX4FMU::set_mode(Mode mode) int PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + for (unsigned pass = 0; pass < 2; pass++) { for (unsigned group = 0; group < _max_actuators; group++) { @@ -323,17 +325,22 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate if (pass == 0) { // preflight if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); // not a legal map, bail return -EINVAL; } } else { // set it - errors here are unexpected if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); return -EINVAL; + } } else { - if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); return -EINVAL; + } } } } @@ -428,6 +435,7 @@ PX4FMU::task_main() _current_update_rate = 50; } + debug("adjusted actuator update interval to %ums", update_rate_in_ms); orb_set_interval(_t_actuators, update_rate_in_ms); } @@ -643,6 +651,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; @@ -1023,9 +1032,6 @@ int fmu_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; - unsigned pwm_rate = 0; - uint32_t alt_channels = 0; - bool alt_channels_set = false; const char *verb = argv[1]; if (fmu_start() != OK) @@ -1041,29 +1047,6 @@ fmu_main(int argc, char *argv[]) new_mode = PORT_FULL_SERIAL; } else if (!strcmp(verb, "mode_pwm")) { - int ch; - char *ap; - - while ((ch = getopt(argc - 1, argv + 1, "c:u:")) != EOF) { - switch (ch) { - case 'u': - pwm_rate = strtol(optarg, nullptr, 0); - break; - - case 'c': - alt_channels = strtol(optarg, &ap, 0); - if (*ap == '\0') - alt_channels_set = true; - break; - - case ':': - errx(1, "missing parameter"); - - default: - errx(1, "unrecognised option"); - } - } - new_mode = PORT_FULL_PWM; } else if (!strcmp(verb, "mode_gpio_serial")) { @@ -1084,19 +1067,8 @@ fmu_main(int argc, char *argv[]) return OK; /* switch modes */ - return fmu_new_mode(new_mode); - - /* if new modes are PWM modes, respect the -u and -c options */ - if ((new_mode == PORT_FULL_PWM) || (new_mode == PORT_PWM_AND_GPIO)) { - if (pwm_rate != 0) { - if (g_fmu->set_pwm_alt_rate(pwm_rate) != OK) - errx(1, "error setting alternate PWM rate"); - } - if (alt_channels_set) { - if (g_fmu->set_pwm_alt_channels(alt_channels)) - errx(1, "serror setting alternate PWM rate channels"); - } - } + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); } if (!strcmp(verb, "test")) @@ -1106,6 +1078,6 @@ fmu_main(int argc, char *argv[]) fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_alt_rate_in_hz] [-c pwm_alt_rate_channel_mask], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); exit(1); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 803e74469..9854402f0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1502,6 +1502,26 @@ namespace { void +start(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->init()) { + delete g_dev; + errx(1, "driver init failed"); + } + + exit(0); +} + +void test(void) { int fd; @@ -1595,34 +1615,8 @@ px4io_main(int argc, char *argv[]) if (argc < 2) goto out; - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) - errx(1, "already loaded"); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(); - - if (g_dev == nullptr) - errx(1, "driver alloc failed"); - - if (OK != g_dev->init()) { - delete g_dev; - errx(1, "driver init failed"); - } - - /* look for the optional pwm update rate for the supported modes */ - if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { - if (argc > 2 + 1) { -#warning implement this - } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); - return 1; - } - } - - exit(0); - } + if (!strcmp(argv[1], "start")) + start(argc - 1, argv + 1); if (!strcmp(argv[1], "recovery")) { diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile new file mode 100644 index 000000000..60885aa0a --- /dev/null +++ b/apps/systemcmds/pwm/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the pwm tool. +# + +APPNAME = pwm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..bf4a1b579 --- /dev/null +++ b/apps/systemcmds/pwm/pwm.c @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pwm.c + * + * PWM servo output configuration and monitoring tool. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int pwm_main(int argc, char *argv[]); + + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <alt_channel_mask>] [arm|disarm] [<channel_value> ...]\n" + " -v Print information about the PWM device\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" + " <alt_channel_mask> Bitmask of channels to update at the alternate rate\n" + " arm | disarm Arm or disarm the ouptut\n" + " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"); +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + uint32_t alt_channels; + bool alt_channels_set = false; + bool print_info = false; + int ch; + int ret; + char *ep; + + while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) { + switch (ch) { + case 'c': + alt_channels = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_channel_mask value"); + alt_channels_set = true; + break; + + case 'd': + dev = optarg; + break; + + case 'u': + alt_rate = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); + + case 'v': + print_info = true; + break; + + default: + usage(NULL); + } + } + argc -= optind; + argv += optind; + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } + + /* assign alternate rate to channels */ + if (alt_channels_set) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)"); + } + + /* iterate remaining arguments */ + unsigned channel = 0; + while (argc--) { + const char *arg = argv[0]; + argv++; + if (!strcmp(arg, "arm")) { + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + continue; + } + if (!strcmp(arg, "disarm")) { + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + continue; + } + unsigned pwm_value = strtol(arg, &ep, 0); + if (*ep == '\0') { + ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", channel); + channel++; + continue; + } + usage("unrecognised option"); + } + + /* print verbose info */ + if (print_info) { + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + printf("PWM output values:\n"); + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("%u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + printf("Available alt_channel_mask groups:\n"); + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) + printf(" 0x%x", group_mask); + } + printf("\n"); + fflush(stdout); + } + exit(0); +}
\ No newline at end of file diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index f1f70e228..80cf6f312 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -67,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check CONFIGURED_APPS += systemcmds/delay_test |