From 811790a14f567b1a58a4434c9fae0cf764e8fd2d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 7 Jan 2013 21:33:04 -0800 Subject: Checkpoint I2C slave work on IO --- apps/systemcmds/i2c/Makefile | 42 +++++++++++++ apps/systemcmds/i2c/i2c.c | 139 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 181 insertions(+) create mode 100644 apps/systemcmds/i2c/Makefile create mode 100644 apps/systemcmds/i2c/i2c.c (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile new file mode 100644 index 000000000..046e74757 --- /dev/null +++ b/apps/systemcmds/i2c/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 i2c test tool. +# + +APPNAME = i2c +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c new file mode 100644 index 000000000..1124e560d --- /dev/null +++ b/apps/systemcmds/i2c/i2c.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 i2c.c + * + * i2c hacking tool + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, no device interface +#endif +#ifndef PX4_I2C_OBDEV_PX4IO +# error PX4_I2C_OBDEV_PX4IO not defined +#endif + +__EXPORT int i2c_main(int argc, char *argv[]); + +static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + +static struct i2c_dev_s *i2c; + +int i2c_main(int argc, char *argv[]) +{ + /* find the right I2C */ + i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + usleep(100000); + + uint32_t val = 0x12345678; + int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); + + if (ret) + errx(1, "transfer failed"); + exit(0); +} + +static int +transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + struct i2c_msg_s msgv[2]; + unsigned msgs; + int ret; + unsigned tries = 0; + + do { + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -1; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(i2c, 320000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + + if (ret == OK) + break; + + // reset the I2C bus to unwedge on error + up_i2creset(i2c); + } while (tries++ < 5); + + return ret; +} -- cgit v1.2.3 From f12fa7ee060b33194723df983b643b51410ea4f6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 8 Jan 2013 01:11:52 -0800 Subject: Don't do retries, since it just complicates things. --- apps/systemcmds/i2c/i2c.c | 73 ++++++++++++++++++++++------------------------- 1 file changed, 34 insertions(+), 39 deletions(-) (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 1124e560d..57c61e824 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -94,46 +94,41 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig struct i2c_msg_s msgv[2]; unsigned msgs; int ret; - unsigned tries = 0; - - do { - // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - - msgs = 0; - - if (send_len > 0) { - msgv[msgs].addr = address; - msgv[msgs].flags = 0; - msgv[msgs].buffer = send; - msgv[msgs].length = send_len; - msgs++; - } - - if (recv_len > 0) { - msgv[msgs].addr = address; - msgv[msgs].flags = I2C_M_READ; - msgv[msgs].buffer = recv; - msgv[msgs].length = recv_len; - msgs++; - } - - if (msgs == 0) - return -1; - - /* - * I2C architecture means there is an unavoidable race here - * if there are any devices on the bus with a different frequency - * preference. Really, this is pointless. - */ - I2C_SETFREQUENCY(i2c, 320000); - ret = I2C_TRANSFER(i2c, &msgv[0], msgs); - - if (ret == OK) - break; - - // reset the I2C bus to unwedge on error + + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -1; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(i2c, 320000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + + // reset the I2C bus to unwedge on error + if (ret != OK) up_i2creset(i2c); - } while (tries++ < 5); return ret; } -- cgit v1.2.3 From 3cea0959b72fe160e6a05e8efef1d325d12d4544 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 9 Jan 2013 21:39:54 -0800 Subject: Implement a simple byte loopback server on I2C for more testing. --- apps/px4io/i2c.c | 9 ++++----- apps/systemcmds/i2c/i2c.c | 9 +++++++-- 2 files changed, 11 insertions(+), 7 deletions(-) (limited to 'apps/systemcmds') diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index e55e992fd..3e4ac3488 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -78,7 +78,6 @@ static DMA_HANDLE tx_dma; uint8_t rx_buf[64]; unsigned rx_len; uint8_t tx_buf[64]; -unsigned tx_len; enum { DIR_NONE = 0, @@ -210,6 +209,9 @@ i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); + for (unsigned i = 0; i < rx_len; i++) + tx_buf[i] = rx_buf[i] + 1; + /* XXX handle reception */ i2c_rx_setup(); } @@ -217,7 +219,6 @@ i2c_rx_complete(DMA_HANDLE handle, uint8_t status, void *arg) static void i2c_tx_setup(void) { - tx_len = 0; stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], sizeof(tx_buf), DMA_CCR_DIR | DMA_CCR_MINC | @@ -229,9 +230,7 @@ i2c_tx_setup(void) static void i2c_tx_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - tx_len = sizeof(tx_buf) - stm32_dmaresidual(tx_dma); - - /* XXX handle reception */ + /* XXX handle transmit-done */ i2c_tx_setup(); } diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 57c61e824..422d9f915 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -84,8 +84,13 @@ int i2c_main(int argc, char *argv[]) int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); if (ret) - errx(1, "transfer failed"); - exit(0); + errx(1, "send failed - %d", ret); + + ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); + if (ret) + errx(1, "recive failed - %d", ret); + + errx(0, "got 0x%08x", val); } static int -- cgit v1.2.3 From d8a013f8720e81afb637b8206fbe521ccb43ac8f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 23 Jan 2013 18:57:16 -0800 Subject: Tinkering. --- apps/systemcmds/i2c/i2c.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c index 422d9f915..e1babdc12 100644 --- a/apps/systemcmds/i2c/i2c.c +++ b/apps/systemcmds/i2c/i2c.c @@ -80,12 +80,13 @@ int i2c_main(int argc, char *argv[]) usleep(100000); - uint32_t val = 0x12345678; - int ret = transfer(PX4_I2C_OBDEV_PX4IO, (uint8_t *)&val, sizeof(val), NULL, 0); + uint8_t buf[] = { 0, 4}; + int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0); if (ret) errx(1, "send failed - %d", ret); + uint32_t val; ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val)); if (ret) errx(1, "recive failed - %d", ret); @@ -128,7 +129,7 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig * if there are any devices on the bus with a different frequency * preference. Really, this is pointless. */ - I2C_SETFREQUENCY(i2c, 320000); + I2C_SETFREQUENCY(i2c, 400000); ret = I2C_TRANSFER(i2c, &msgv[0], msgs); // reset the I2C bus to unwedge on error -- cgit v1.2.3 From b526bab1748f8a74202caf42966a5a719bba28ad Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 4 Mar 2013 21:46:55 -0800 Subject: Remove extra spaces from mixers before processing them. This gives us some more working space on IO for mixer processing. --- apps/systemcmds/mixer/mixer.c | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd5..55c4f0836 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) -- cgit v1.2.3 From 7011fe563b583d7940247a7a01265b7f8675fee8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 12 Mar 2013 00:40:22 -0700 Subject: Move PWM rate configuration, etc. into a separate utility and out of the individual drivers. --- apps/drivers/hil/hil.cpp | 25 +---- apps/drivers/px4fmu/fmu.cpp | 56 +++------- apps/drivers/px4io/px4io.cpp | 50 ++++----- apps/systemcmds/pwm/Makefile | 42 ++++++++ apps/systemcmds/pwm/pwm.c | 207 +++++++++++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 6 files changed, 289 insertions(+), 92 deletions(-) create mode 100644 apps/systemcmds/pwm/Makefile create mode 100644 apps/systemcmds/pwm/pwm.c (limited to 'apps/systemcmds') 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 @@ -1501,6 +1501,26 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); 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) { @@ -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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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 ] [-u ] [-c ] [arm|disarm] [ ...]\n" + " -v Print information about the PWM device\n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " PWM update rate for channels in \n" + " Bitmask of channels to update at the alternate rate\n" + " arm | disarm Arm or disarm the ouptut\n" + " ... 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 -- cgit v1.2.3 From 6e71466aee0b07c5ff85b40e9fb76aa92187df92 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 17 Mar 2013 12:29:18 -0700 Subject: Adjust the syntax of the 'pwm' command to make it easier to use. --- apps/systemcmds/pwm/Makefile | 2 +- apps/systemcmds/pwm/pwm.c | 58 ++++++++++++++++++++++++++++++++------------ 2 files changed, 43 insertions(+), 17 deletions(-) (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile index 60885aa0a..5ab105ed1 100644 --- a/apps/systemcmds/pwm/Makefile +++ b/apps/systemcmds/pwm/Makefile @@ -37,6 +37,6 @@ APPNAME = pwm PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c index bf4a1b579..de7a53199 100644 --- a/apps/systemcmds/pwm/pwm.c +++ b/apps/systemcmds/pwm/pwm.c @@ -71,13 +71,17 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d ] [-u ] [-c ] [arm|disarm] [ ...]\n" + "pwm [-v] [-d ] [-u ] [-c ] [arm|disarm] [ ...]\n" " -v Print information about the PWM device\n" " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" " PWM update rate for channels in \n" - " Bitmask of channels to update at the alternate rate\n" + " Channel group that should update at the alternate rate (may be specified more than once)\n" " arm | disarm Arm or disarm the ouptut\n" - " ... PWM output values in microseconds to assign to the PWM outputs\n"); + " ... PWM output values in microseconds to assign to the PWM outputs\n" + "\n" + "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + ); + } int @@ -85,19 +89,24 @@ pwm_main(int argc, char *argv[]) { const char *dev = PWM_OUTPUT_DEVICE_PATH; unsigned alt_rate = 0; - uint32_t alt_channels; + uint32_t alt_channel_groups = 0; bool alt_channels_set = false; bool print_info = false; int ch; int ret; char *ep; + unsigned group; + + if (argc < 2) + usage(NULL); 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"); + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); alt_channels_set = true; break; @@ -109,6 +118,7 @@ pwm_main(int argc, char *argv[]) alt_rate = strtol(optarg, &ep, 0); if (*ep != '\0') usage("bad alt_rate value"); + break; case 'v': print_info = true; @@ -133,11 +143,25 @@ pwm_main(int argc, char *argv[]) err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); } - /* assign alternate rate to channels */ + /* assign alternate rate to channel groups */ if (alt_channels_set) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels); + uint32_t mask = 0; + + for (unsigned group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)"); + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); } /* iterate remaining arguments */ @@ -177,30 +201,32 @@ pwm_main(int argc, char *argv[]) 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); + printf("channel %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); + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } } - printf("\n"); fflush(stdout); } exit(0); -- cgit v1.2.3 From 9849d22e4fec1d21cc77c54fec658cfaf16f8185 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Mar 2013 11:05:38 +0100 Subject: Added MAVLink-transmitted calibration warning about bad sensor calibration as part of preflight check --- apps/systemcmds/preflight_check/preflight_check.c | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'apps/systemcmds') diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c index f25f812ae..9ac6f0b5e 100644 --- a/apps/systemcmds/preflight_check/preflight_check.c +++ b/apps/systemcmds/preflight_check/preflight_check.c @@ -54,6 +54,8 @@ #include #include +#include + __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); static int led_on(int leds, int led); @@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[]) bool system_ok = true; int fd; + /* open text message output path */ + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; /* give the system some time to sample the sensors in the background */ @@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[]) fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); system_ok = false; goto system_eval; } @@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION"); system_ok = false; goto system_eval; } @@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("accel self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); system_ok = false; goto system_eval; } @@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[]) if (ret != OK) { warnx("gyro self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); system_ok = false; goto system_eval; } -- cgit v1.2.3