From 2289c0bb216027419a9ba5e52103a1310ff03292 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 11:30:41 +0200 Subject: Moved all system commands to new build system --- src/systemcmds/bl_update/bl_update.c | 215 ++++++++++++ src/systemcmds/bl_update/module.mk | 43 +++ src/systemcmds/boardinfo/.context | 0 src/systemcmds/boardinfo/boardinfo.c | 409 +++++++++++++++++++++++ src/systemcmds/boardinfo/module.mk | 41 +++ src/systemcmds/eeprom/module.mk | 33 ++ src/systemcmds/i2c/i2c.c | 140 ++++++++ src/systemcmds/i2c/module.mk | 41 +++ src/systemcmds/mixer/mixer.c | 152 +++++++++ src/systemcmds/mixer/module.mk | 43 +++ src/systemcmds/param/module.mk | 44 +++ src/systemcmds/param/param.c | 297 ++++++++++++++++ src/systemcmds/perf/.context | 0 src/systemcmds/perf/module.mk | 41 +++ src/systemcmds/perf/perf.c | 81 +++++ src/systemcmds/preflight_check/module.mk | 42 +++ src/systemcmds/preflight_check/preflight_check.c | 206 ++++++++++++ src/systemcmds/pwm/module.mk | 41 +++ src/systemcmds/pwm/pwm.c | 233 +++++++++++++ src/systemcmds/reboot/.context | 0 src/systemcmds/reboot/module.mk | 41 +++ src/systemcmds/reboot/reboot.c | 53 +++ src/systemcmds/top/.context | 0 src/systemcmds/top/module.mk | 44 +++ src/systemcmds/top/top.c | 253 ++++++++++++++ 25 files changed, 2493 insertions(+) create mode 100644 src/systemcmds/bl_update/bl_update.c create mode 100644 src/systemcmds/bl_update/module.mk create mode 100644 src/systemcmds/boardinfo/.context create mode 100644 src/systemcmds/boardinfo/boardinfo.c create mode 100644 src/systemcmds/boardinfo/module.mk create mode 100644 src/systemcmds/i2c/i2c.c create mode 100644 src/systemcmds/i2c/module.mk create mode 100644 src/systemcmds/mixer/mixer.c create mode 100644 src/systemcmds/mixer/module.mk create mode 100644 src/systemcmds/param/module.mk create mode 100644 src/systemcmds/param/param.c create mode 100644 src/systemcmds/perf/.context create mode 100644 src/systemcmds/perf/module.mk create mode 100644 src/systemcmds/perf/perf.c create mode 100644 src/systemcmds/preflight_check/module.mk create mode 100644 src/systemcmds/preflight_check/preflight_check.c create mode 100644 src/systemcmds/pwm/module.mk create mode 100644 src/systemcmds/pwm/pwm.c create mode 100644 src/systemcmds/reboot/.context create mode 100644 src/systemcmds/reboot/module.mk create mode 100644 src/systemcmds/reboot/reboot.c create mode 100644 src/systemcmds/top/.context create mode 100644 src/systemcmds/top/module.mk create mode 100644 src/systemcmds/top/top.c (limited to 'src/systemcmds') diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c new file mode 100644 index 000000000..0569d89f5 --- /dev/null +++ b/src/systemcmds/bl_update/bl_update.c @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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 bl_update.c + * + * STM32F4 bootloader update tool. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +__EXPORT int bl_update_main(int argc, char *argv[]); + +static void setopt(void); + +int +bl_update_main(int argc, char *argv[]) +{ + if (argc != 2) + errx(1, "missing firmware filename or command"); + + if (!strcmp(argv[1], "setopt")) + setopt(); + + int fd = open(argv[1], O_RDONLY); + + if (fd < 0) + err(1, "open %s", argv[1]); + + struct stat s; + + if (stat(argv[1], &s) < 0) + err(1, "stat %s", argv[1]); + + /* sanity-check file size */ + if (s.st_size > 16384) + errx(1, "%s: file too large", argv[1]); + + uint8_t *buf = malloc(s.st_size); + + if (buf == NULL) + errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size); + + if (read(fd, buf, s.st_size) != s.st_size) + err(1, "firmware read error"); + + close(fd); + + uint32_t *hdr = (uint32_t *)buf; + + if ((hdr[0] < 0x20000000) || /* stack not below RAM */ + (hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */ + (hdr[1] < 0x08000000) || /* entrypoint not below flash */ + ((hdr[1] - 0x08000000) > 16384)) { /* entrypoint not outside bootloader */ + free(buf); + errx(1, "not a bootloader image"); + } + + warnx("image validated, erasing bootloader..."); + usleep(10000); + + /* prevent other tasks from running while we do this */ + sched_lock(); + + /* unlock the control register */ + volatile uint32_t *keyr = (volatile uint32_t *)0x40023c04; + *keyr = 0x45670123U; + *keyr = 0xcdef89abU; + + volatile uint32_t *sr = (volatile uint32_t *)0x40023c0c; + volatile uint32_t *cr = (volatile uint32_t *)0x40023c10; + volatile uint8_t *base = (volatile uint8_t *)0x08000000; + + /* check the control register */ + if (*cr & 0x80000000) { + warnx("WARNING: flash unlock failed, flash aborted"); + goto flash_end; + } + + /* erase the bootloader sector */ + *cr = 0x2; + *cr = 0x10002; + + /* wait for the operation to complete */ + while (*sr & 0x1000) { + } + + if (*sr & 0xf2) { + warnx("WARNING: erase error 0x%02x", *sr); + goto flash_end; + } + + /* verify the erase */ + for (int i = 0; i < s.st_size; i++) { + if (base[i] != 0xff) { + warnx("WARNING: erase failed at %d - retry update, DO NOT reboot", i); + goto flash_end; + } + } + + warnx("flashing..."); + + /* now program the bootloader - speed is not critical so use x8 mode */ + for (int i = 0; i < s.st_size; i++) { + + /* program a byte */ + *cr = 1; + base[i] = buf[i]; + + /* wait for the operation to complete */ + while (*sr & 0x1000) { + } + + if (*sr & 0xf2) { + warnx("WARNING: program error 0x%02x", *sr); + goto flash_end; + } + } + + /* re-lock the flash control register */ + *cr = 0x80000000; + + warnx("verifying..."); + + /* now run a verify pass */ + for (int i = 0; i < s.st_size; i++) { + if (base[i] != buf[i]) { + warnx("WARNING: verify failed at %u - retry update, DO NOT reboot", i); + goto flash_end; + } + } + + warnx("bootloader update complete"); + +flash_end: + /* unlock the scheduler */ + sched_unlock(); + + free(buf); + exit(0); +} + +static void +setopt(void) +{ + volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14; + + const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */ + const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */ + + if ((*optcr & opt_mask) == opt_bits) + errx(0, "option bits are already set as required"); + + /* unlock the control register */ + volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08; + *optkeyr = 0x08192a3bU; + *optkeyr = 0x4c5d6e7fU; + + if (*optcr & 1) + errx(1, "option control register unlock failed"); + + /* program the new option value */ + *optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1); + + usleep(1000); + + if ((*optcr & opt_mask) == opt_bits) + errx(0, "option bits set"); + + errx(1, "option bits setting failed; readback 0x%04x", *optcr); + +} \ No newline at end of file diff --git a/src/systemcmds/bl_update/module.mk b/src/systemcmds/bl_update/module.mk new file mode 100644 index 000000000..e8eea045e --- /dev/null +++ b/src/systemcmds/bl_update/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Bootloader updater (flashes the FMU USB bootloader software) +# + +MODULE_COMMAND = bl_update +SRCS = bl_update.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/boardinfo/.context b/src/systemcmds/boardinfo/.context new file mode 100644 index 000000000..e69de29bb diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c new file mode 100644 index 000000000..3ff5a066c --- /dev/null +++ b/src/systemcmds/boardinfo/boardinfo.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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 boardinfo.c + * autopilot and carrier board information app + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +__EXPORT int boardinfo_main(int argc, char *argv[]); + +struct eeprom_info_s +{ + unsigned bus; + unsigned address; + unsigned page_size; + unsigned page_count; + unsigned page_write_delay; +}; + +/* XXX currently code below only supports 8-bit addressing */ +const struct eeprom_info_s eeprom_info[] = { + {3, 0x57, 8, 16, 3300}, +}; + +struct board_parameter_s { + const char *name; + bson_type_t type; +}; + +const struct board_parameter_s board_parameters[] = { + {"name", BSON_STRING}, /* ascii board name */ + {"vers", BSON_INT32}, /* board version (major << 8) | minor */ + {"date", BSON_INT32}, /* manufacture date */ + {"build", BSON_INT32} /* build code (fab << 8) | tester */ +}; + +const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); + +static int +eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; + + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; + } + I2C_SETFREQUENCY(dev, 400000); + + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { + + uint8_t pagebuf[eeprom->page_size + 1]; + + /* how many bytes available to transfer? */ + unsigned count = size - address; + + /* constrain writes to the page size */ + if (count > eeprom->page_size) + count = eeprom->page_size; + + pagebuf[0] = address & 0xff; + memcpy(pagebuf + 1, buf + address, count); + + struct i2c_msg_s msgv[1] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = pagebuf, + .length = count + 1 + } + }; + + result = I2C_TRANSFER(dev, msgv, 1); + if (result != OK) { + warnx("EEPROM write failed: %d", result); + goto out; + } + usleep(eeprom->page_write_delay); + address += count; + } + +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} + +static int +eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) +{ + int result = -1; + + struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); + if (dev == NULL) { + warnx("failed to init bus %d for EEPROM", eeprom->bus); + goto out; + } + I2C_SETFREQUENCY(dev, 400000); + + /* loop until all data has been transferred */ + for (unsigned address = 0; address < size; ) { + + /* how many bytes available to transfer? */ + unsigned count = size - address; + + /* constrain transfers to the page size (bus anti-hog) */ + if (count > eeprom->page_size) + count = eeprom->page_size; + + uint8_t addr = address; + struct i2c_msg_s msgv[2] = { + { + .addr = eeprom->address, + .flags = 0, + .buffer = &addr, + .length = 1 + }, + { + .addr = eeprom->address, + .flags = I2C_M_READ, + .buffer = buf + address, + .length = count + } + }; + + result = I2C_TRANSFER(dev, msgv, 2); + if (result != OK) { + warnx("EEPROM read failed: %d", result); + goto out; + } + address += count; + } + +out: + if (dev != NULL) + up_i2cuninitialize(dev); + return result; +} + +static void * +idrom_read(const struct eeprom_info_s *eeprom) +{ + uint32_t size = 0xffffffff; + int result; + void *buf = NULL; + + result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); + if (result != 0) { + warnx("failed reading ID ROM length"); + goto fail; + } + if (size > (eeprom->page_size * eeprom->page_count)) { + warnx("ID ROM not programmed"); + goto fail; + } + + buf = malloc(size); + if (buf == NULL) { + warnx("could not allocate %d bytes for ID ROM", size); + goto fail; + } + result = eeprom_read(eeprom, buf, size); + if (result != 0) { + warnx("failed reading ID ROM"); + goto fail; + } + return buf; + +fail: + if (buf != NULL) + free(buf); + return NULL; +} + +static void +boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) +{ + struct bson_encoder_s encoder; + int result = 1; + char *state, *token; + unsigned i; + + /* create the encoder and make a writable copy of the spec */ + bson_encoder_init_buf(&encoder, NULL, 0); + + for (i = 0, token = strtok_r(spec, ",", &state); + token && (i < num_parameters); + i++, token = strtok_r(NULL, ",", &state)) { + + switch (board_parameters[i].type) { + case BSON_STRING: + result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); + break; + case BSON_INT32: + result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); + break; + default: + result = 1; + } + if (result) { + warnx("bson append failed for %s<%s>", board_parameters[i].name, token); + goto out; + } + } + bson_encoder_fini(&encoder); + if (i != num_parameters) { + warnx("incorrect parameter list, expected: \",,\""); + result = 1; + goto out; + } + if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { + warnx("data too large for EEPROM"); + result = 1; + goto out; + } + if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { + warnx("buffer length mismatch"); + result = 1; + goto out; + } + warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + + result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); + if (result < 0) { + warnc(-result, "error writing EEPROM"); + result = 1; + } else { + result = 0; + } + +out: + free(bson_encoder_buf_data(&encoder)); + + exit(result); +} + +static int +boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) +{ + switch (node->type) { + case BSON_INT32: + printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); + break; + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + printf("%s: %s\n", node->name, buf); + break; + } + case BSON_EOO: + break; + default: + warnx("unexpected node type %d", node->type); + break; + } + return 1; +} + +static void +boardinfo_show(const struct eeprom_info_s *eeprom) +{ + struct bson_decoder_s decoder; + void *buf; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { + while (bson_decoder_next(&decoder) > 0) + ; + } else { + warnx("failed to init decoder"); + } + free(buf); + exit(0); +} + +struct { + const char *property; + const char *value; +} test_args; + +static int +boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + /* reject nodes with non-matching names */ + if (strcmp(node->name, test_args.property)) + return 1; + + /* compare node values to check for a match */ + switch (node->type) { + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + + /* check for a match */ + if (!strcmp(test_args.value, buf)) { + return 2; + } + break; + } + + case BSON_INT32: { + int32_t val = strtol(test_args.value, NULL, 0); + + /* check for a match */ + if (node->i == val) { + return 2; + } + break; + } + + default: + break; + } + + return 1; +} + +static void +boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) +{ + struct bson_decoder_s decoder; + void *buf; + int result = -1; + + if ((property == NULL) || (strlen(property) == 0) || + (value == NULL) || (strlen(value) == 0)) + errx(1, "missing property name or value"); + + test_args.property = property; + test_args.value = value; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { + do { + result = bson_decoder_next(&decoder); + } while (result == 1); + } else { + warnx("failed to init decoder"); + } + free(buf); + + /* if we matched, we exit with zero success */ + exit((result == 2) ? 0 : 1); +} + +int +boardinfo_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "set")) + boardinfo_set(&eeprom_info[0], argv[2]); + + if (!strcmp(argv[1], "show")) + boardinfo_show(&eeprom_info[0]); + + if (!strcmp(argv[1], "test")) + boardinfo_test(&eeprom_info[0], argv[2], argv[3]); + + errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); +} diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk new file mode 100644 index 000000000..6851d229b --- /dev/null +++ b/src/systemcmds/boardinfo/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Information about FMU and IO boards connected +# + +MODULE_COMMAND = boardinfo +SRCS = boardinfo.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk index 3b4fc0479..07f3945be 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/systemcmds/eeprom/module.mk @@ -1,3 +1,36 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + # # EEPROM file system driver # diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c new file mode 100644 index 000000000..4da238039 --- /dev/null +++ b/src/systemcmds/i2c/i2c.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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); + + 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); + + errx(0, "got 0x%08x", val); +} + +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; + + // 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, 400000); + ret = I2C_TRANSFER(i2c, &msgv[0], msgs); + + // reset the I2C bus to unwedge on error + if (ret != OK) + up_i2creset(i2c); + + return ret; +} diff --git a/src/systemcmds/i2c/module.mk b/src/systemcmds/i2c/module.mk new file mode 100644 index 000000000..ab2357c7d --- /dev/null +++ b/src/systemcmds/i2c/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Build the i2c test tool. +# + +MODULE_COMMAND = i2c +SRCS = i2c.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c new file mode 100644 index 000000000..55c4f0836 --- /dev/null +++ b/src/systemcmds/mixer/mixer.c @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mixer.c + * + * Mixer utility. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +__EXPORT int mixer_main(int argc, char *argv[]); + +static void usage(const char *reason) noreturn_function; +static void load(const char *devname, const char *fname) noreturn_function; + +int +mixer_main(int argc, char *argv[]) +{ + if (argc < 2) + usage("missing command"); + + if (!strcmp(argv[1], "load")) { + if (argc < 4) + usage("missing device or filename"); + + load(argv[2], argv[3]); + } + + usage("unrecognised command"); +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage:\n"); + fprintf(stderr, " mixer load \n"); + /* XXX other useful commands? */ + exit(1); +} + +static void +load(const char *devname, const char *fname) +{ + int dev; + FILE *fp; + char line[80]; + char buf[512]; + + /* open the device */ + if ((dev = open(devname, 0)) < 0) + err(1, "can't open %s\n", devname); + + /* reset mixers on the device */ + if (ioctl(dev, MIXERIOCRESET, 0)) + err(1, "can't reset mixers on %s", devname); + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* 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)) + break; + + /* add the line to the buffer */ + strcat(buf, line); + } + + /* XXX pass the buffer to the device */ + int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); + + if (ret < 0) + err(1, "error loading mixers from %s", fname); + exit(0); +} diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk new file mode 100644 index 000000000..d5a3f9f77 --- /dev/null +++ b/src/systemcmds/mixer/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Build the mixer tool. +# + +MODULE_COMMAND = mixer +SRCS = mixer.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk new file mode 100644 index 000000000..63f15ad28 --- /dev/null +++ b/src/systemcmds/param/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Build the parameters tool. +# + +MODULE_COMMAND = param +SRCS = param.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os + diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c new file mode 100644 index 000000000..56f5317e3 --- /dev/null +++ b/src/systemcmds/param/param.c @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * 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 param.c + * + * Parameter tool. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int param_main(int argc, char *argv[]); + +static void do_save(const char* param_file_name); +static void do_load(const char* param_file_name); +static void do_import(const char* param_file_name); +static void do_show(const char* search_string); +static void do_show_print(void *arg, param_t param); +static void do_set(const char* name, const char* val); + +int +param_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "save")) { + if (argc >= 3) { + do_save(argv[2]); + } else { + do_save(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "load")) { + if (argc >= 3) { + do_load(argv[2]); + } else { + do_load(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "import")) { + if (argc >= 3) { + do_import(argv[2]); + } else { + do_import(param_get_default_file()); + } + } + + if (!strcmp(argv[1], "select")) { + if (argc >= 3) { + param_set_default_file(argv[2]); + } else { + param_set_default_file(NULL); + } + warnx("selected parameter default file %s", param_get_default_file()); + exit(0); + } + + if (!strcmp(argv[1], "show")) { + if (argc >= 3) { + do_show(argv[2]); + } else { + do_show(NULL); + } + } + + if (!strcmp(argv[1], "set")) { + if (argc >= 4) { + do_set(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); + } + } + } + + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); +} + +static void +do_save(const char* param_file_name) +{ + /* delete the parameter file in case it exists */ + unlink(param_file_name); + + /* create the file */ + int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", param_file_name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(param_file_name); + errx(1, "error exporting to '%s'", param_file_name); + } + + exit(0); +} + +static void +do_load(const char* param_file_name) +{ + int fd = open(param_file_name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", param_file_name); + + int result = param_load(fd); + close(fd); + + if (result < 0) { + errx(1, "error importing from '%s'", param_file_name); + } + + exit(0); +} + +static void +do_import(const char* param_file_name) +{ + int fd = open(param_file_name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", param_file_name); + + int result = param_import(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", param_file_name); + + exit(0); +} + +static void +do_show(const char* search_string) +{ + printf(" + = saved, * = unsaved\n"); + param_foreach(do_show_print, search_string, false); + + exit(0); +} + +static void +do_show_print(void *arg, param_t param) +{ + int32_t i; + float f; + const char *search_string = (const char*)arg; + + /* print nothing if search string is invalid and not matching */ + if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { + /* param not found */ + return; + } + + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + + /* + * This case can be expanded to handle printing common structure types. + */ + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("%d\n", i); + return; + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("%4.4f\n", (double)f); + return; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + printf("\n", 0 + param_type(param), param_size(param)); + return; + + default: + printf("\n", 0 + param_type(param)); + return; + } + + printf("\n", param); +} + +static void +do_set(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + printf("%c %s: ", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("old: %d", i); + + /* convert string */ + char* end; + i = strtol(val,&end,10); + param_set(param, &i); + printf(" -> new: %d\n", i); + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("float values are not yet supported."); + // printf("old: %4.4f", (double)f); + + // /* convert string */ + // char* end; + // f = strtof(val,&end); + // param_set(param, &f); + // printf(" -> new: %4.4f\n", f); + + } + + break; + + default: + errx(1, "\n", 0 + param_type(param)); + } + + exit(0); +} diff --git a/src/systemcmds/perf/.context b/src/systemcmds/perf/.context new file mode 100644 index 000000000..e69de29bb diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk new file mode 100644 index 000000000..77952842b --- /dev/null +++ b/src/systemcmds/perf/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# perf_counter reporting tool +# + +MODULE_COMMAND = perf +SRCS = perf.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c new file mode 100644 index 000000000..b69ea597b --- /dev/null +++ b/src/systemcmds/perf/perf.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int perf_main(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int perf_main(int argc, char *argv[]) +{ + if (argc > 1) { + if (strcmp(argv[1], "reset") == 0) { + perf_reset_all(); + return 0; + } + printf("Usage: perf \n"); + return -1; + } + + perf_print_all(); + fflush(stdout); + return 0; +} + + diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk new file mode 100644 index 000000000..7c3c88783 --- /dev/null +++ b/src/systemcmds/preflight_check/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Pre-flight check. Locks down system for a few systems with blinking leds +# and buzzer if the sensors do not report an OK status. +# + +MODULE_COMMAND = preflight_check +SRCS = preflight_check.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c new file mode 100644 index 000000000..42256be61 --- /dev/null +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: @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 reboot.c + * Tool similar to UNIX reboot command + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#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); +static int led_off(int leds, int led); + +int preflight_check_main(int argc, char *argv[]) +{ + bool fail_on_error = false; + + if (argc > 1 && !strcmp(argv[1], "--help")) { + warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); + exit(1); + } + + if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { + fail_on_error = true; + } + + 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 */ + usleep(150000); + + + /* ---- MAG ---- */ + close(fd); + 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; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); + + 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; + } + + /* ---- ACCEL ---- */ + + close(fd); + fd = open(ACCEL_DEVICE_PATH, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + warnx("accel self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK"); + system_ok = false; + goto system_eval; + } + + /* ---- GYRO ---- */ + + close(fd); + fd = open(GYRO_DEVICE_PATH, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + warnx("gyro self test failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK"); + system_ok = false; + goto system_eval; + } + + /* ---- BARO ---- */ + + close(fd); + fd = open(BARO_DEVICE_PATH, 0); + + + +system_eval: + + if (system_ok) { + /* all good, exit silently */ + exit(0); + } else { + fflush(stdout); + + int buzzer = open("/dev/tone_alarm", O_WRONLY); + int leds = open(LED_DEVICE_PATH, 0); + + /* flip blue led into alternating amber */ + led_off(leds, LED_BLUE); + led_off(leds, LED_AMBER); + led_toggle(leds, LED_BLUE); + + /* display and sound error */ + for (int i = 0; i < 150; i++) + { + led_toggle(leds, LED_BLUE); + led_toggle(leds, LED_AMBER); + + if (i % 10 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 4); + } else if (i % 5 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 2); + } + usleep(100000); + } + + /* stop alarm */ + ioctl(buzzer, TONE_SET_ALARM, 0); + + /* switch on leds */ + led_on(leds, LED_BLUE); + led_on(leds, LED_AMBER); + + if (fail_on_error) { + /* exit with error message */ + exit(1); + } else { + /* do not emit an error code to make sure system still boots */ + exit(0); + } + } +} + +static int led_toggle(int leds, int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_off(int leds, int led) +{ + return ioctl(leds, LED_OFF, led); +} + +static int led_on(int leds, int led) +{ + return ioctl(leds, LED_ON, led); +} \ No newline at end of file diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk new file mode 100644 index 000000000..4a23bba90 --- /dev/null +++ b/src/systemcmds/pwm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Build the pwm tool. +# + +MODULE_COMMAND = pwm +SRCS = pwm.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..de7a53199 --- /dev/null +++ b/src/systemcmds/pwm/pwm.c @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * 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" + " 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" + "\n" + "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + ); + +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + 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': + 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; + + case 'd': + dev = optarg; + break; + + case 'u': + alt_rate = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); + break; + + 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 channel groups */ + if (alt_channels_set) { + 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"); + } + + /* 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 */ + 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("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + 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("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + fflush(stdout); + } + exit(0); +} \ No newline at end of file diff --git a/src/systemcmds/reboot/.context b/src/systemcmds/reboot/.context new file mode 100644 index 000000000..e69de29bb diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk new file mode 100644 index 000000000..19f64af54 --- /dev/null +++ b/src/systemcmds/reboot/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# reboot command. +# + +MODULE_COMMAND = reboot +SRCS = reboot.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c new file mode 100644 index 000000000..47d3cd948 --- /dev/null +++ b/src/systemcmds/reboot/reboot.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @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 reboot.c + * Tool similar to UNIX reboot command + */ + +#include +#include +#include + +#include + +__EXPORT int reboot_main(int argc, char *argv[]); + +int reboot_main(int argc, char *argv[]) +{ + up_systemreset(); +} + + diff --git a/src/systemcmds/top/.context b/src/systemcmds/top/.context new file mode 100644 index 000000000..e69de29bb diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk new file mode 100644 index 000000000..9239aafc3 --- /dev/null +++ b/src/systemcmds/top/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Build top memory / cpu status tool. +# + +MODULE_COMMAND = top +SRCS = top.c + +MODULE_STACKSIZE = 3000 + +MAXOPTIMIZATION = -Os + diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c new file mode 100644 index 000000000..59d2bc8f1 --- /dev/null +++ b/src/systemcmds/top/top.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @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 top.c + * Tool similar to UNIX top command + * @see http://en.wikipedia.org/wiki/Top_unix + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +/** + * Start the top application. + */ +__EXPORT int top_main(int argc, char *argv[]); + +extern struct system_load_s system_load; + +bool top_sigusr1_rcvd = false; + +int top_main(int argc, char *argv[]) +{ + int t; + + uint64_t total_user_time = 0; + + int running_count = 0; + int blocked_count = 0; + + uint64_t new_time = hrt_absolute_time(); + uint64_t interval_start_time = new_time; + + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + + for (t = 0; t < CONFIG_MAX_TASKS; t++) + last_times[t] = 0; + + float interval_time_ms_inv = 0.f; + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + + while (true) +// for (t = 0; t < 10; t++) + { + int i; + + uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU); + unsigned int curr_time_s = curr_time_ms / 1000LLU; + + uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU); + unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU; + + if (new_time > interval_start_time) + interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + + running_count = 0; + blocked_count = 0; + total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0; + + last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + curr_loads[i] = interval_runtime * interval_time_ms_inv; + + if (i > 0) + total_user_time += interval_runtime; + + } else + curr_loads[i] = 0; + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle = curr_loads[0]; + float task_load = (float)(total_user_time) * interval_time_ms_inv; + + if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ + + float sched_load = 1.f - idle - task_load; + + /* print system information */ + printf("\033[H"); /* cursor home */ + printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count); + printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100)); + printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000)); + + /* 34 chars command name length (32 chars plus two spaces) */ + char header_spaces[CONFIG_TASK_NAME_SIZE + 1]; + memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); + header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; +#if CONFIG_RR_INTERVAL > 0 + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); +#else + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces); +#endif + + } else { + enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state; + + if (task_state == TSTATE_TASK_PENDING || + task_state == TSTATE_TASK_READYTORUN || + task_state == TSTATE_TASK_RUNNING) { + running_count++; + } + + if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */ + task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */ +#ifndef CONFIG_DISABLE_SIGNALS + || task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */ +#endif +#ifndef CONFIG_DISABLE_MQUEUE + || task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */ + || task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */ +#endif +#ifdef CONFIG_PAGING + || task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */ +#endif + ) { + blocked_count++; + } + + char spaces[CONFIG_TASK_NAME_SIZE + 2]; + + /* count name len */ + int namelen = 0; + + while (namelen < CONFIG_TASK_NAME_SIZE) { + if (system_load.tasks[i].tcb->name[namelen] == '\0') break; + + namelen++; + } + + int s = 0; + + for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) { + spaces[s] = ' '; + } + + spaces[s] = '\0'; + + char *runtime_spaces = " "; + + if ((system_load.tasks[i].total_runtime / 1000) < 99) { + runtime_spaces = ""; + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) + break; + + stack_free++; + } + + printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u", + (int)system_load.tasks[i].tcb->pid, + system_load.tasks[i].tcb->name, + spaces, + (system_load.tasks[i].total_runtime / 1000), + runtime_spaces, + (int)(curr_loads[i] * 100), + (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + stack_size - stack_free, + stack_size); + /* Print scheduling info with RR time slice */ +#if CONFIG_RR_INTERVAL > 0 + printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); +#else + /* Print scheduling info without time slice*/ + printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority); +#endif + } + } + } + + printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J"); + fflush(stdout); + + interval_start_time = new_time; + + char c; + + /* Sleep 200 ms waiting for user input four times */ + /* XXX use poll ... */ + for (int k = 0; k < 4; k++) { + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + printf("Abort\n"); + close(console); + return OK; + } + } + + usleep(200000); + } + + new_time = hrt_absolute_time(); + } + + close(console); + + return OK; +} -- cgit v1.2.3