From 137afdbd3cf8c9356fabf15e8acbb41661daf40b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Oct 2012 19:11:27 +0100 Subject: Remove excessive mem usage --- apps/commander/commander.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4b49201c7..e00dc13d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -363,6 +363,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) if (x == NULL || y == NULL || z == NULL) { warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + printf("x:%p y:%p z:%p\n", x, y, z); return; } @@ -1101,7 +1102,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 9000, + 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; -- cgit v1.2.3 From b685d46dbfc583a26a92f1466dca64f9d45c3c4e Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Oct 2012 20:51:45 -0700 Subject: Clean out remains of the old EEPROM driver. --- apps/drivers/boards/px4fmu/px4fmu_init.c | 8 +- apps/px4/tests/test_eeproms.c | 328 ------------------- apps/px4/tests/tests_main.c | 1 - apps/systemlib/systemlib.c | 1 - nuttx/configs/px4fmu/include/drv_eeprom.h | 60 ---- nuttx/configs/px4fmu/src/Makefile | 2 +- nuttx/configs/px4fmu/src/drv_eeprom.c | 522 ------------------------------ 7 files changed, 5 insertions(+), 917 deletions(-) delete mode 100644 apps/px4/tests/test_eeproms.c delete mode 100644 nuttx/configs/px4fmu/include/drv_eeprom.h delete mode 100644 nuttx/configs/px4fmu/src/drv_eeprom.c diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 2dc3e60c6..38a284017 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -65,7 +65,6 @@ #include #include -#include #include @@ -220,6 +219,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); +#if 0 /* initialize I2C2 bus */ i2c2 = up_i2cinitialize(2); @@ -250,7 +250,7 @@ __EXPORT int nsh_archinitialize(void) FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); - +#endif #if defined(CONFIG_STM32_SPI3) /* Get the SPI port */ @@ -276,7 +276,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); #endif /* SPI3 */ - +#if 0 /* initialize I2C1 bus */ i2c1 = up_i2cinitialize(1); @@ -293,7 +293,7 @@ __EXPORT int nsh_archinitialize(void) /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ /* Get board information if available */ - +#endif #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c deleted file mode 100644 index 29ca8267f..000000000 --- a/apps/px4/tests/test_eeproms.c +++ /dev/null @@ -1,328 +0,0 @@ -/**************************************************************************** - * px4/eeproms/test_eeproms.c - * - * 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 NuttX 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 -#include -#include -#include -#include - -#include - -#include "tests.h" - -#include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -static int onboard_eeprom(int argc, char *argv[]); -static int baseboard_eeprom(int argc, char *argv[]); - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -struct { - const char *name; - const char *path; - int (* test)(int argc, char *argv[]); -} eeproms[] = { - {"onboard_eeprom", "/dev/eeprom", onboard_eeprom}, - {"baseboard_eeprom", "/dev/baseboard_eeprom", baseboard_eeprom}, - {NULL, NULL, NULL} -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -static int -onboard_eeprom(int argc, char *argv[]) -{ - printf("\tonboard_eeprom: test start\n"); - fflush(stdout); - - int fd; - uint8_t buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '}; - int ret; - bool force_write = false; - if (strcmp(argv[0], "jig") == 0) force_write = true; - - /* fill with spaces */ - //memset(buf1+16, 'x', sizeof(buf1-16)); - - /* fill in some magic values at magic positions */ - buf1[63] = 'E'; - buf1[64] = 'S'; - buf1[127] = 'F'; - buf1[128] = 'T'; - - /* terminate string */ - buf1[sizeof(buf1) - 1] = '\0'; - - fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); - - if (fd < 0) { - printf("onboard eeprom: open fail\n"); - return ERROR; - } - - /* read data */ - ret = read(fd, buf1, 1); - - if (ret != 1) { - printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret); - - switch (-ret) { - case EPERM: - printf("\treason: %s\n", EPERM_STR); - break; - - case ENOENT: - printf("\treason: %s\n", ENOENT_STR); - break; - - case ESRCH: - printf("\treason: %s\n", ESRCH_STR); - break; - - case EINTR: - printf("\treason: %s\n", EINTR_STR); - break; - - } - } - - printf("\tonboard eeprom: first byte: %d\n", buf1[0]); - if (!force_write) { - printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n"); - - printf("Input: "); - char c = getchar(); - printf("%c\n", c); - if (c != 'y' && c != 'Y') { - /* not yes, abort */ - close(fd); - - /* Let user know everything is ok */ - printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n"); - return OK; - } - } - - printf("\tonboard eeprom: proceeding with write test\r\n"); - - /* increment counter */ - buf1[0] = buf1[0] + 1; - - /* rewind to the start of the file */ - lseek(fd, 0, SEEK_SET); - - /* write data */ - ret = write(fd, buf1, sizeof(buf1)); - - if (ret != sizeof(buf1)) { - printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret); - - switch (-ret) { - case EPERM: - printf("\treason: %s\n", EPERM_STR); - break; - - case ENOENT: - printf("\treason: %s\n", ENOENT_STR); - break; - - case ESRCH: - printf("\treason: %s\n", ESRCH_STR); - break; - - case EINTR: - printf("\treason: %s\n", EINTR_STR); - break; - - } - - //return ERROR; - } - - /* rewind to the start of the file */ - lseek(fd, 0, SEEK_SET); - - /* read data */ - ret = read(fd, buf1, sizeof(buf1)); - - if (ret != sizeof(buf1)) { - printf("\tonboard eeprom: ERROR: read fail: %d\n", ret); - - switch (-ret) { - case EPERM: - printf("\treason: %s\n", EPERM_STR); - break; - - case ENOENT: - printf("\treason: %s\n", ENOENT_STR); - break; - - case ESRCH: - printf("\treason: %s\n", ESRCH_STR); - break; - - case EINTR: - printf("\treason: %s\n", EINTR_STR); - break; - - } - - return ERROR; - - } else { - /* enforce null termination and print as string */ - if (buf1[sizeof(buf1) - 1] != 0) { - printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n"); - buf1[sizeof(buf1) - 1] = '\0'; - } - - /* read out counter and replace val */ - int counter = buf1[0]; - printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1); - printf("\tAll %d bytes:\n\n\t", sizeof(buf1)); - - for (int i = 0; i < sizeof(buf1); i++) { - printf("0x%02x ", buf1[i]); - - if (i % 8 == 7) printf("\n\t"); - - if (i % 64 == 63) printf("\n\t"); - } - - /* end any open line */ - printf("\n\n"); - } - - close(fd); - - /* Let user know everything is ok */ - printf("\tOK: onboard eeprom passed all tests successfully\n"); - return ret; -} - -static int -baseboard_eeprom(int argc, char *argv[]) -{ - printf("\tbaseboard eeprom: test start\n"); - fflush(stdout); - - int fd; - uint8_t buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'}; - int ret; - - fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK); - - if (fd < 0) { - printf("\tbaseboard eeprom: open fail\n"); - return ERROR; - } - - /* read data */ - ret = read(fd, buf, sizeof(buf)); - /* set last char to string termination */ - buf[127] = '\0'; - - if (ret != sizeof(buf)) { - printf("\tbaseboard eeprom: ERROR: read fail\n", ret); - return ERROR; - - } else { - printf("\tbaseboard eeprom: string: %s\n", (char *)buf); - } - - close(fd); - - /* XXX more tests here */ - - /* Let user know everything is ok */ - printf("\tOK: baseboard eeprom passed all tests successfully\n"); - return ret; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_eeproms - ****************************************************************************/ - -int test_eeproms(int argc, char *argv[]) -{ - unsigned i; - - printf("Running EEPROMs tests:\n\n"); - fflush(stdout); - - for (i = 0; eeproms[i].name; i++) { - printf(" eeprom: %s\n", eeproms[i].name); - eeproms[i].test(argc, argv); - fflush(stdout); - /* wait 100 ms to make sure buffer is emptied */ - usleep(100000); - } - - return 0; -} diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c index b9f6835b0..26f7ef96b 100644 --- a/apps/px4/tests/tests_main.c +++ b/apps/px4/tests/tests_main.c @@ -97,7 +97,6 @@ struct { {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"adc", test_adc, OPT_NOJIGTEST, 0}, {"jig_voltages", test_jig_voltages, OPT_NOALLTEST, 0}, - {"eeproms", test_eeproms, 0, 0}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index b596b0f0e..84ba809a3 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -45,7 +45,6 @@ #include #include #include -#include #include #include diff --git a/nuttx/configs/px4fmu/include/drv_eeprom.h b/nuttx/configs/px4fmu/include/drv_eeprom.h deleted file mode 100644 index 57589ed84..000000000 --- a/nuttx/configs/px4fmu/include/drv_eeprom.h +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * 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 drv_eeprom.h - * - * Config for the non-MTD EEPROM driver. - */ - -/* IMPORTANT: Adjust this number! */ -#define MAX_EEPROMS 2 - -/* FMU onboard */ -#define FMU_BASEBOARD_EEPROM_ADDRESS 0x57 -#define FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES 128 -#define FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES 8 -#define FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US 3300 -#define FMU_BASEBOARD_EEPROM_BUS_CLOCK 400000 ///< 400 KHz max. clock - -/** - * @brief i2c I2C bus struct - * @brief device_address The device address as stated in the datasheet, e.g. for a Microchip 24XX128 0x50 with all ID pins tied to GND - * @brief total_size_bytes The total size in bytes, e.g. 16K = 16000 bytes for the Microchip 24XX128 - * @brief page_size_bytes The size of one page, e.g. 64 bytes for the Microchip 24XX128 - * @brief device_name The device name to register this device to, e.g. /dev/eeprom - * @brief fail_if_missing Returns error if the EEPROM was not found. This is helpful if the EEPROM might be attached later when the board is running - */ -extern int -eeprom_attach(struct i2c_dev_s *i2c, uint8_t device_address, uint16_t total_size_bytes, uint16_t page_size_bytes, uint16_t page_write_time_us, const char* device_name, uint8_t fail_if_missing); - diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 13b26b84f..42c931112 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -41,7 +41,7 @@ ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_leds.c \ - drv_led.c drv_eeprom.c + drv_led.c COBJS = $(CSRCS:.c=$(OBJEXT)) diff --git a/nuttx/configs/px4fmu/src/drv_eeprom.c b/nuttx/configs/px4fmu/src/drv_eeprom.c deleted file mode 100644 index c22062ec5..000000000 --- a/nuttx/configs/px4fmu/src/drv_eeprom.c +++ /dev/null @@ -1,522 +0,0 @@ -/* - * Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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. - */ - -/* - * Generic driver for I2C EEPROMs with 8 bit or 16 bit addressing - */ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -#include - -/* Split I2C transfers into smaller chunks to make sure to stay within tight timeout limits */ - -/* check defines */ -#ifndef MAX_EEPROMS - #error MAX_EEPROMS number must be defined (1-3) -#endif - -#if (MAX_EEPROMS > 3) - #error Currently only a maximum of three EEPROMS is supported, add missing code around here: __FILE__:__LINE__ -#endif -static int eeprom_open0(FAR struct file *filp); -static int eeprom_close0(FAR struct file *filp); -static ssize_t eeprom_read0(struct file *filp, FAR char *buffer, size_t buflen); -static ssize_t eeprom_write0(struct file *filp, FAR const char *buffer, size_t buflen); -static off_t eeprom_seek0(FAR struct file *filp, off_t offset, int whence); -#if (MAX_EEPROMS > 1) -static int eeprom_open1(FAR struct file *filp); -static int eeprom_close1(FAR struct file *filp); -static ssize_t eeprom_read1(struct file *filp, FAR char *buffer, size_t buflen); -static ssize_t eeprom_write1(struct file *filp, FAR const char *buffer, size_t buflen); -static off_t eeprom_seek1(FAR struct file *filp, off_t offset, int whence); -#endif -#if (MAX_EEPROMS > 2) -static int eeprom_open2(FAR struct file *filp); -static int eeprom_close2(FAR struct file *filp); -static ssize_t eeprom_read2(struct file *filp, FAR char *buffer, size_t buflen); -static ssize_t eeprom_write2(struct file *filp, FAR const char *buffer, size_t buflen); -static off_t eeprom_seek2(FAR struct file *filp, off_t offset, int whence); -#endif - -static const struct file_operations eeprom_fops[MAX_EEPROMS] = {{ - .open = eeprom_open0, - .close = eeprom_close0, - .read = eeprom_read0, - .write = eeprom_write0, - .seek = eeprom_seek0, - .ioctl = 0, -#ifndef CONFIG_DISABLE_POLL - .poll = 0 -#endif -} -#if (MAX_EEPROMS > 1) -,{ - .open = eeprom_open1, - .close = eeprom_close1, - .read = eeprom_read1, - .write = eeprom_write1, - .seek = eeprom_seek1, -} -#endif -#if (MAX_EEPROMS > 2) -,{ - .open = eeprom_open2, - .close = eeprom_close2, - .read = eeprom_read2, - .write = eeprom_write2, - .seek = eeprom_seek2, -} -#endif -}; - -static FAR struct eeprom_dev_s -{ - struct i2c_dev_s *i2c; - uint8_t eeprom_address; - uint16_t eeprom_size_bytes; - uint16_t eeprom_page_size_bytes; - uint16_t eeprom_page_write_time; - off_t offset; - bool is_open; -} eeprom_dev[MAX_EEPROMS]; - -static int -eeprom_open0(FAR struct file *filp) -{ - /* only allow one open at a time */ - if (eeprom_dev[0].is_open) { - errno = EBUSY; - return -EBUSY; - } - /* reset pointer */ - //eeprom_dev[0].is_open = true; - eeprom_dev[0].offset = 0; - return OK; -} -#if (MAX_EEPROMS > 1) -static int -eeprom_open1(FAR struct file *filp) -{ - /* only allow one open at a time */ - if (eeprom_dev[1].is_open) { - errno = EBUSY; - return -EBUSY; - } - /* reset pointer */ - //eeprom_dev[1].is_open = true; - eeprom_dev[1].offset = 0; - return OK; -} -#endif -#if (MAX_EEPROMS > 2) -static int -eeprom_open2(FAR struct file *filp) -{ - /* only allow one open at a time */ - if (eeprom_dev[2].is_open) { - errno = EBUSY; - return -EBUSY; - } - /* reset pointer */ - //eeprom_dev[2].is_open = true; - eeprom_dev[2].offset = 0; - return OK; -} -#endif - -static int -eeprom_close0(FAR struct file *filp) -{ - eeprom_dev[0].is_open = false; - return OK; -} -#if (MAX_EEPROMS > 1) -static int -eeprom_close1(FAR struct file *filp) -{ - eeprom_dev[1].is_open = false; - return OK; -} -#endif -#if (MAX_EEPROMS > 2) -static int -eeprom_close2(FAR struct file *filp) -{ - eeprom_dev[2].is_open = false; - return OK; -} -#endif - -static int -eeprom_read_internal(int dev, uint16_t len, uint8_t *data) -{ - /* abort if the number of requested bytes exceeds the EEPROM size */ - if (eeprom_dev[dev].offset + len > eeprom_dev[dev].eeprom_size_bytes) - { - errno = ENOSPC; - return -ENOSPC; - } - - /* set device address */ - I2C_SETADDRESS(eeprom_dev[dev].i2c, eeprom_dev[dev].eeprom_address, 7); - - uint8_t cmd[2] = {0, 0}; /* first (or only) part of address */ - /* second part of address, omitted if eeprom has 256 bytes or less */ - int ret = 0; - int remaining = len; - int readcounts = 0; - - while (remaining > 0) - { - /* read all requested bytes over potentially multiple pages */ - //int readlen = (remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes; - int read_offset = eeprom_dev[dev].offset + len - remaining;//+ write_counts*eeprom_dev[dev].eeprom_page_size_bytes; - /* set read length to page border */ - int readlen = eeprom_dev[dev].eeprom_page_size_bytes - (read_offset % eeprom_dev[dev].eeprom_page_size_bytes);//(remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes; - /* cap read length if not a full page read is needed */ - if (readlen > remaining) readlen = remaining; - - if (eeprom_dev[dev].eeprom_size_bytes <= 256) - { - cmd[0] = (read_offset); /* set at first byte */ - /* 8 bit addresses */ - ret = I2C_WRITEREAD(eeprom_dev[dev].i2c, cmd, 1, (data+(readcounts*eeprom_dev[dev].eeprom_page_size_bytes)), readlen); - } - else - { - /* 16 bit addresses */ - /* EEPROM: first address high, then address low */ - cmd[0] = (((uint16_t)read_offset) >> 8); - cmd[1] = (((uint8_t)read_offset)); - ret = I2C_WRITEREAD(eeprom_dev[dev].i2c, cmd, 2, (data+(readcounts*eeprom_dev[dev].eeprom_page_size_bytes)), readlen); - } - - /* abort on error */ - if (ret < 0) break; - - /* handled another chunk */ - remaining -= readlen; - readcounts++; - } - - /* use the negated value from I2C_TRANSFER to fill errno */ - errno = -ret; - - /* return len if data was read, < 0 else */ - if (ret == OK) - eeprom_dev[dev].offset += len; - return len; - - /* no data, return negated value from I2C_TRANSFER */ - return ret; -} - -static int -eeprom_write_internal(int dev, uint16_t len, const uint8_t *data) -{ - /* abort if the number of requested bytes exceeds the EEPROM size */ - if (eeprom_dev[dev].offset + len > eeprom_dev[dev].eeprom_size_bytes) - { - errno = ENOSPC; - return -ENOSPC; - } - - int ret = 0; - int remaining = len; - int write_counts = 0; - - uint8_t write_buf[2]; - - while (remaining > 0) - { - /* write all requested bytes over potentially multiple pages */ - int write_offset = eeprom_dev[dev].offset + len - remaining;//+ write_counts*eeprom_dev[dev].eeprom_page_size_bytes; - /* set write length to page border */ - int writelen = eeprom_dev[dev].eeprom_page_size_bytes - (write_offset % eeprom_dev[dev].eeprom_page_size_bytes);//(remaining < eeprom_dev[dev].eeprom_page_size_bytes) ? remaining : eeprom_dev[dev].eeprom_page_size_bytes; - /* cap write length if not a full page write is requested */ - if (writelen > remaining) writelen = remaining; - - if (eeprom_dev[dev].eeprom_size_bytes <= 256) - { - write_buf[0] = (write_offset); /* set at first byte */ - /* 8 bit addresses */ - - const uint8_t* data_ptr = (data+(write_offset)); - - struct i2c_msg_s msgv_eeprom_write[2] = { - { - .addr = eeprom_dev[dev].eeprom_address, - .flags = I2C_M_NORESTART, - .buffer = write_buf, - .length = 1 - }, - { - .addr = eeprom_dev[dev].eeprom_address, - .flags = I2C_M_NORESTART, - .buffer = (uint8_t*)data_ptr, - .length = writelen - } - }; - - - if ( (ret = I2C_TRANSFER(eeprom_dev[dev].i2c, msgv_eeprom_write, 2)) == OK ) - { - //printf("SUCCESS WRITING EEPROM 8BIT ADDR: %d, bytes: %d\n", ret, writelen); - } - } - else - { - /* 16 bit addresses */ - /* EEPROM: first address high, then address low */ - write_buf[0] = (((uint16_t)write_offset) >> 8); - write_buf[1] = (((uint8_t)write_offset)); - - const uint8_t* data_ptr = data+(write_counts*eeprom_dev[dev].eeprom_page_size_bytes); - - struct i2c_msg_s msgv_eeprom_write[2] = { - { - .addr = eeprom_dev[dev].eeprom_address, - .flags = I2C_M_NORESTART, - .buffer = write_buf, - .length = 2 - }, - { - .addr = eeprom_dev[dev].eeprom_address, - .flags = I2C_M_NORESTART, - .buffer = (uint8_t*)data_ptr, - .length = writelen - } - }; - - - if ( (ret = I2C_TRANSFER(eeprom_dev[dev].i2c, msgv_eeprom_write, 2)) == OK ) - { - //printf("SUCCESS WRITING EEPROM 16BIT ADDR: %d, bytes: %d\n", ret, writelen); - } - } - - /* abort on error */ - if (ret < 0) break; - - /* handled another chunk */ - remaining -= writelen; - write_counts++; - /* wait for the device to write the page */ - usleep(eeprom_dev[dev].eeprom_page_write_time); - } - - /* use the negated value from I2C_TRANSFER to fill errno */ - errno = -ret; - - /* return length if data was written, < 0 else */ - if (ret == OK) - eeprom_dev[dev].offset += len; - return len; - - /* no data, return negated value from I2C_TRANSFER */ - return ret; -} - -static ssize_t -eeprom_read0(struct file *filp, char *buffer, size_t buflen) -{ - return eeprom_read_internal(0, buflen, (uint8_t *)buffer); -} -#if (MAX_EEPROMS > 1) -static ssize_t -eeprom_read1(struct file *filp, char *buffer, size_t buflen) -{ - return eeprom_read_internal(1, buflen, (uint8_t *)buffer); -} -#endif -#if (MAX_EEPROMS > 2) -static ssize_t -eeprom_read2(struct file *filp, char *buffer, size_t buflen) -{ - return eeprom_read_internal(2, buflen, (uint8_t *)buffer); -} -#endif - -static ssize_t -eeprom_write0(struct file *filp, const char *buffer, size_t buflen) -{ - return eeprom_write_internal(0, buflen, (const uint8_t *)buffer); -} -#if (MAX_EEPROMS > 1) -static ssize_t -eeprom_write1(struct file *filp, const char *buffer, size_t buflen) -{ - return eeprom_write_internal(1, buflen, (const uint8_t *)buffer); -} -#endif -#if (MAX_EEPROMS > 2) -static ssize_t -eeprom_write2(struct file *filp, const char *buffer, size_t buflen) -{ - return eeprom_write_internal(2, buflen, (const uint8_t *)buffer); -} -#endif - -static off_t eeprom_seek0(FAR struct file *filp, off_t offset, int whence) -{ - switch (whence) - { - case SEEK_SET: - if (offset < eeprom_dev[0].eeprom_size_bytes - 1) { - eeprom_dev[0].offset = offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_CUR: - if (eeprom_dev[0].offset + offset < eeprom_dev[0].eeprom_size_bytes - 1) { - eeprom_dev[0].offset = eeprom_dev[0].offset + offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_END: - errno = ESPIPE; - return -ESPIPE; - break; - } - return eeprom_dev[0].offset; -} -#if (MAX_EEPROMS > 1) -static off_t eeprom_seek1(FAR struct file *filp, off_t offset, int whence) -{ - switch (whence) - { - case SEEK_SET: - if (offset < eeprom_dev[1].eeprom_size_bytes - 1) { - eeprom_dev[1].offset = offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_CUR: - if (eeprom_dev[1].offset + offset < eeprom_dev[1].eeprom_size_bytes - 1) { - eeprom_dev[1].offset = eeprom_dev[1].offset + offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_END: - errno = ESPIPE; - return -ESPIPE; - break; - } - return eeprom_dev[1].offset; -} -#endif -#if (MAX_EEPROMS > 2) -static off_t eeprom_seek2(FAR struct file *filp, off_t offset, int whence) -{ - switch (whence) - { - case SEEK_SET: - if (offset < eeprom_dev[2].eeprom_size_bytes - 1) { - eeprom_dev[2].offset = offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_CUR: - if (eeprom_dev[2].offset + offset < eeprom_dev[2].eeprom_size_bytes - 1) { - eeprom_dev[2].offset = eeprom_dev[2].offset + offset; - } else { - errno = ESPIPE; - return -ESPIPE; - } - break; - case SEEK_END: - errno = ESPIPE; - return -ESPIPE; - break; - } - return eeprom_dev[2].offset; -} -#endif - -int -eeprom_attach(struct i2c_dev_s *i2c, uint8_t device_address, uint16_t total_size_bytes, uint16_t page_size_bytes, uint16_t page_write_time_us, const char* device_name, uint8_t fail_if_missing) -{ - static int eeprom_dev_counter = 0; - eeprom_dev[eeprom_dev_counter].i2c = i2c; - eeprom_dev[eeprom_dev_counter].eeprom_address = device_address; - eeprom_dev[eeprom_dev_counter].eeprom_size_bytes = total_size_bytes; - eeprom_dev[eeprom_dev_counter].eeprom_page_size_bytes = page_size_bytes; - eeprom_dev[eeprom_dev_counter].eeprom_page_write_time = page_write_time_us; - eeprom_dev[eeprom_dev_counter].offset = 0; - eeprom_dev[eeprom_dev_counter].is_open = false; - - int ret; - - if (fail_if_missing) { - /* read first value */ - uint8_t read_test; - ret = (eeprom_read_internal(eeprom_dev_counter, 1, &read_test) == 1) ? OK : ERROR; - } else { - ret = OK; - } - - /* make ourselves available */ - if (ret == OK) - { - register_driver(device_name, &(eeprom_fops[eeprom_dev_counter]), 0666, NULL); - eeprom_dev_counter++; - } - - /* Return 0 for device found, error number else */ - return ret; -} -- cgit v1.2.3 From 34a3b260f34398994a315388b3ffed22a1fe22fb Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 31 Oct 2012 00:37:15 -0700 Subject: Move the last of the board-specific code for PX4FMU out of the NuttX tree. Now it's just configuration. --- apps/commander/commander.c | 4 +- apps/drivers/boards/px4fmu/px4fmu_init.c | 64 +------- apps/drivers/boards/px4fmu/px4fmu_led.c | 88 +++++++++++ apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 10 +- apps/drivers/drv_led.h | 64 ++++++++ apps/drivers/led/Makefile | 38 +++++ apps/drivers/led/led.cpp | 115 ++++++++++++++ apps/px4/tests/test_float.c | 2 - apps/px4/tests/test_int.c | 2 - apps/px4/tests/test_led.c | 4 +- apps/px4/tests/test_sleep.c | 2 - apps/px4/tests/test_uart_baudchange.c | 2 - apps/px4/tests/test_uart_console.c | 2 - apps/px4/tests/test_uart_loopback.c | 2 - apps/px4/tests/test_uart_send.c | 2 - apps/px4/tests/tests.h | 1 - apps/systemcmds/led/Makefile | 42 ------ apps/systemcmds/led/led.c | 209 -------------------------- nuttx/configs/px4fmu/include/drv_led.h | 51 ------- nuttx/configs/px4fmu/nsh/appconfig | 2 +- nuttx/configs/px4fmu/src/Makefile | 5 +- nuttx/configs/px4fmu/src/drv_led.c | 113 -------------- nuttx/configs/px4fmu/src/empty.c | 4 + nuttx/configs/px4fmu/src/px4fmu-internal.h | 166 -------------------- nuttx/configs/px4fmu/src/up_leds.c | 127 ---------------- 25 files changed, 322 insertions(+), 799 deletions(-) create mode 100644 apps/drivers/boards/px4fmu/px4fmu_led.c create mode 100644 apps/drivers/drv_led.h create mode 100644 apps/drivers/led/Makefile create mode 100644 apps/drivers/led/led.cpp delete mode 100644 apps/systemcmds/led/Makefile delete mode 100644 apps/systemcmds/led/led.c delete mode 100644 nuttx/configs/px4fmu/include/drv_led.h delete mode 100644 nuttx/configs/px4fmu/src/drv_led.c create mode 100644 nuttx/configs/px4fmu/src/empty.c delete mode 100644 nuttx/configs/px4fmu/src/px4fmu-internal.h delete mode 100644 nuttx/configs/px4fmu/src/up_leds.c diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e00dc13d2..bf9d07c9b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -61,7 +61,7 @@ #include #include #include -#include +#include #include #include #include @@ -194,7 +194,7 @@ static void buzzer_deinit() static int led_init() { - leds = open("/dev/led", O_RDONLY | O_NONBLOCK); + leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { fprintf(stderr, "[commander] LED: open fail\n"); diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 38a284017..568d861c9 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -64,9 +64,9 @@ #include "stm32_uart.h" #include -#include #include +#include #include @@ -131,9 +131,6 @@ __EXPORT void stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi3; -static struct i2c_dev_s *i2c1; -static struct i2c_dev_s *i2c2; -static struct i2c_dev_s *i2c3; #include @@ -153,10 +150,6 @@ __EXPORT int nsh_archinitialize(void) { int result; - /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ - - /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ - /* configure the high-resolution time/callout interface */ #ifdef CONFIG_HRT_TIMER hrt_init(); @@ -190,14 +183,12 @@ __EXPORT int nsh_archinitialize(void) message("\r\n"); + // initial LED state + drv_led_start(); up_ledoff(LED_BLUE); up_ledoff(LED_AMBER); - up_ledon(LED_BLUE); - /* Configure user-space led driver */ - px4fmu_led_init(); - /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); @@ -219,38 +210,6 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if 0 - /* initialize I2C2 bus */ - - i2c2 = up_i2cinitialize(2); - - if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C2 speed */ - I2C_SETFREQUENCY(i2c2, 400000); - - - i2c3 = up_i2cinitialize(3); - - if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C3 speed */ - I2C_SETFREQUENCY(i2c3, 400000); - - /* try to attach, don't fail if device is not responding */ - (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, - FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); -#endif #if defined(CONFIG_STM32_SPI3) /* Get the SPI port */ @@ -276,24 +235,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); #endif /* SPI3 */ -#if 0 - /* initialize I2C1 bus */ - - i2c1 = up_i2cinitialize(1); - if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C1 speed */ - I2C_SETFREQUENCY(i2c1, 400000); - - /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ - - /* Get board information if available */ -#endif #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/drivers/boards/px4fmu/px4fmu_led.c b/apps/drivers/boards/px4fmu/px4fmu_led.c new file mode 100644 index 000000000..fd1e159aa --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_led.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 px4fmu_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include +#include +#include + +#include + +#include "chip.h" +#include "up_arch.h" +#include "up_internal.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +__EXPORT void up_ledinit() +{ + /* Configure LED1-2 GPIOs for output */ + + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); +} + +__EXPORT void up_ledon(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED2, false); + } +} + +__EXPORT void up_ledoff(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c index d1ff8c112..cb8918306 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -46,13 +46,9 @@ #include #include -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include +#include +#include __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h new file mode 100644 index 000000000..bf21787f2 --- /dev/null +++ b/apps/drivers/drv_led.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 drv_led.h + * + * LED driver API + */ + +#pragma once + +#include +#include + +#define LED_DEVICE_PATH "/dev/led" + +#define _LED_BASE 0x2800 + +/* PX4 LED colour codes */ +#define LED_AMBER 0 +#define LED_RED 0 /* some boards have red rather than amber */ +#define LED_BLUE 1 + +#define LED_ON _IOC(_LED_BASE, 0) +#define LED_OFF _IOC(_LED_BASE, 1) + +__BEGIN_DECLS + +/* + * Initialise the LED driver. + */ +__EXPORT extern void drv_led_start(); + +__END_DECLS diff --git a/apps/drivers/led/Makefile b/apps/drivers/led/Makefile new file mode 100644 index 000000000..7de188259 --- /dev/null +++ b/apps/drivers/led/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the LED driver. +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp new file mode 100644 index 000000000..12d864be2 --- /dev/null +++ b/apps/drivers/led/led.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 led.cpp + * + * LED driver. + */ + +#include +#include +#include + +/* Ideally we'd be able to get these from up_internal.h */ +//#include +__BEGIN_DECLS +extern void up_ledinit(); +extern void up_ledon(int led); +extern void up_ledoff(int led); +__END_DECLS + +class LED : device::CDev +{ +public: + LED(); + ~LED(); + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); +}; + +LED::LED() : + CDev("led", LED_DEVICE_PATH) +{ + // force immediate init/device registration + init(); +} + +LED::~LED() +{ +} + +int +LED::init() +{ + CDev::init(); + up_ledinit(); + + return 0; +} + +int +LED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + switch (cmd) { + case LED_ON: + up_ledon(arg); + break; + + case LED_OFF: + up_ledoff(arg); + break; + + default: + result = CDev::ioctl(filp, cmd, arg); + } + return result; +} + +namespace +{ +LED *gLED; +} + +void +drv_led_start() +{ + if (gLED == nullptr) { + gLED = new LED; + if (gLED != nullptr) + gLED->init(); + } +} \ No newline at end of file diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c index 8c70b720b..4921c9bbb 100644 --- a/apps/px4/tests/test_float.c +++ b/apps/px4/tests/test_float.c @@ -45,8 +45,6 @@ #include #include #include -#include -#include #include "tests.h" #include #include diff --git a/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c index 40e030641..c59cee7b7 100644 --- a/apps/px4/tests/test_int.c +++ b/apps/px4/tests/test_int.c @@ -49,8 +49,6 @@ #include -#include - #include "tests.h" #include diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c index 53615ccd8..6e3efc668 100644 --- a/apps/px4/tests/test_led.c +++ b/apps/px4/tests/test_led.c @@ -49,7 +49,7 @@ #include -#include +#include #include "tests.h" @@ -91,7 +91,7 @@ int test_led(int argc, char *argv[]) int fd; int ret = 0; - fd = open("/dev/led", O_RDONLY | O_NONBLOCK); + fd = open(LED_DEVICE_PATH, 0); if (fd < 0) { printf("\tLED: open fail\n"); diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c index 911a9c2e1..c7b9d2833 100644 --- a/apps/px4/tests/test_sleep.c +++ b/apps/px4/tests/test_sleep.c @@ -49,8 +49,6 @@ #include -#include - #include "tests.h" /**************************************************************************** diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c index 06965bd3d..609a65c62 100644 --- a/apps/px4/tests/test_uart_baudchange.c +++ b/apps/px4/tests/test_uart_baudchange.c @@ -52,8 +52,6 @@ #include -#include - #include "tests.h" #include diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c index fc5b03036..f8582b52f 100644 --- a/apps/px4/tests/test_uart_console.c +++ b/apps/px4/tests/test_uart_console.c @@ -50,8 +50,6 @@ #include -#include - #include "tests.h" #include diff --git a/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c index b2e07df1c..3be152004 100644 --- a/apps/px4/tests/test_uart_loopback.c +++ b/apps/px4/tests/test_uart_loopback.c @@ -50,8 +50,6 @@ #include -#include - #include "tests.h" #include diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c index f5c26e9f3..7e1e8d307 100644 --- a/apps/px4/tests/test_uart_send.c +++ b/apps/px4/tests/test_uart_send.c @@ -50,8 +50,6 @@ #include -#include - #include "tests.h" #include diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h index 1023f5f51..46450d10b 100644 --- a/apps/px4/tests/tests.h +++ b/apps/px4/tests/tests.h @@ -84,7 +84,6 @@ extern int test_led(int argc, char *argv[]); extern int test_adc(int argc, char *argv[]); extern int test_int(int argc, char *argv[]); extern int test_float(int argc, char *argv[]); -extern int test_eeproms(int argc, char *argv[]); extern int test_ppm(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); extern int test_uart_loopback(int argc, char *argv[]); diff --git a/apps/systemcmds/led/Makefile b/apps/systemcmds/led/Makefile deleted file mode 100644 index eb9d8f909..000000000 --- a/apps/systemcmds/led/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Makefile to build ardrone interface -# - -APPNAME = led -PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c deleted file mode 100644 index 15d448118..000000000 --- a/apps/systemcmds/led/led.c +++ /dev/null @@ -1,209 +0,0 @@ -/**************************************************************************** - * - * 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 led.c - * Plain, stupid led outputs - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -__EXPORT int led_main(int argc, char *argv[]); - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int led_task; /**< Handle of deamon task / thread */ -static int leds; - -static int led_init(void) -{ - leds = open("/dev/led", O_RDONLY | O_NONBLOCK); - - if (leds < 0) { - errx(1, "[led] LED: open fail\n"); - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - errx(1, "[led] LED: ioctl fail\n"); - } - - return 0; -} - -static void led_deinit(void) -{ - close(leds); -} - -static int led_toggle(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_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -/** - * Mainloop of led. - */ -int led_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: led {start|stop|status} [-d ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int led_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("led already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - led_task = task_spawn("led", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 4096, - led_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tled is running\n"); - - } else { - printf("\tled not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int led_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - printf("[led] Control started, taking over motors\n"); - - /* open leds */ - led_init(); - - unsigned int rate = 200; - - while (!thread_should_exit) { - /* swell blue led */ - - - /* 200 Hz base loop */ - usleep(1000000 / rate); - } - - /* close leds */ - led_deinit(); - - printf("[led] ending now...\n\n"); - fflush(stdout); - - thread_running = false; - - return OK; -} - diff --git a/nuttx/configs/px4fmu/include/drv_led.h b/nuttx/configs/px4fmu/include/drv_led.h deleted file mode 100644 index 4b7093346..000000000 --- a/nuttx/configs/px4fmu/include/drv_led.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#include - -#define _LEDCBASE 0x6800 -#define LEDC(_x) _IOC(_LEDCBASE, _x) - -/* set the LED identified by the argument */ -#define LED_ON LEDC(1) - -/* clear the LED identified by the argument */ -#define LED_OFF LEDC(2) - -///* toggle the LED identified by the argument */ -//#define LED_TOGGLE LEDC(3) - -#define LED_BLUE 0 /* Led on first port */ -#define LED_AMBER 1 /* Led on second port */ - -extern int px4fmu_led_init(void); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 4da9065df..5b2546911 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -52,7 +52,6 @@ CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom -CONFIGURED_APPS += systemcmds/led CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/bl_update #CONFIGURED_APPS += systemcmds/calibration @@ -94,6 +93,7 @@ CONFIGURED_APPS += drivers/bma180 CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 +CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/px4fmu diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 42c931112..c3d6bf543 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -40,9 +40,7 @@ CFLAGS += -I$(TOPDIR)/sched ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) -CSRCS = up_leds.c \ - drv_led.c - +CSRCS = empty.c COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) @@ -69,6 +67,7 @@ libboard$(LIBEXT): $(OBJS) @( for obj in $(OBJS) ; do \ $(call ARCHIVE, $@, $${obj}); \ done ; ) + touch $@ .depend: Makefile $(SRCS) @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep diff --git a/nuttx/configs/px4fmu/src/drv_led.c b/nuttx/configs/px4fmu/src/drv_led.c deleted file mode 100644 index 13d8eb22a..000000000 --- a/nuttx/configs/px4fmu/src/drv_led.c +++ /dev/null @@ -1,113 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/* - * led driver for PX4FMU - * - * This is something of an experiment currently (ha, get it?) - */ - -#include - -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -#include - -static int px4fmu_led_ioctl(struct file *filep, int cmd, unsigned long arg); -static ssize_t px4fmu_led_pseudoread(struct file *filp, FAR char *buffer, size_t buflen); - -static const struct file_operations px4fmu_led_fops = { - .read = px4fmu_led_pseudoread, - .ioctl = px4fmu_led_ioctl, -}; - -int -px4fmu_led_init(void) -{ - /* register the driver */ - return register_driver("/dev/led", &px4fmu_led_fops, 0666, NULL); -} - -static ssize_t -px4fmu_led_pseudoread(struct file *filp, FAR char *buffer, size_t buflen) -{ - return 0; -} - -static int -px4fmu_led_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - int result = 0; - - switch (cmd) { - - case LED_ON: - switch (arg) { - case 0: - case 1: - up_ledon(arg); - break; - default: - result = -1; - break; - } - break; - - case LED_OFF: - switch (arg) { - case 0: - case 1: - up_ledoff(arg); - break; - default: - result = -1; - break; - } - break; - default: - result = -1; - break; - } - return result; -} - diff --git a/nuttx/configs/px4fmu/src/empty.c b/nuttx/configs/px4fmu/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx/configs/px4fmu/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx/configs/px4fmu/src/px4fmu-internal.h b/nuttx/configs/px4fmu/src/px4fmu-internal.h deleted file mode 100644 index f48b1bf5e..000000000 --- a/nuttx/configs/px4fmu/src/px4fmu-internal.h +++ /dev/null @@ -1,166 +0,0 @@ -/**************************************************************************************************** - * configs/px4fmu/src/px4fmu_internal.h - * arch/arm/src/board/px4fmu_internal.n - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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. - * - ****************************************************************************************************/ - -#ifndef __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H -#define __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -#ifdef CONFIG_STM32_SPI2 -# error "SPI2 is not supported on this board" -#endif - -#if defined(CONFIG_STM32_CAN1) -# warning "CAN1 is not supported on this board" -#endif - -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) - -/* External interrupts */ -#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) -// XXX MPU6000 DRDY? - -/* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) -#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) -#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) - -/* User GPIOs - * - * GPIO0-1 are the buffered high-power GPIOs. - * GPIO2-5 are the USART2 pins. - * GPIO6-7 are the CAN2 pins. - */ -#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) -#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) -#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) -#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) -#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) -#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) -#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) -#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) -#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) -#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) -#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) -#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) - -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) - */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) - -/* PWM - * - * The PX4FMU has five PWM outputs, of which only the output on - * pin PC8 is fixed assigned to this function. The other four possible - * pwm sources are the TX, RX, RTS and CTS pins of USART2 - * - * Alternate function mapping: - * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 - */ - -#ifdef CONFIG_PWM -# if defined(CONFIG_STM32_TIM3_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 3 -# elif defined(CONFIG_STM32_TIM8_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 8 -# endif -#endif - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - -/**************************************************************************************************** - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ****************************************************************************************************/ - -extern void stm32_spiinitialize(void); - -/**************************************************************************************************** - * Name: px4fmu_gpio_init - * - * Description: - * Called to configure the PX4FMU user GPIOs - * - ****************************************************************************************************/ - -extern void px4fmu_gpio_init(void); - - -// XXX additional SPI chipselect functions required? - -#endif /* __ASSEMBLY__ */ -#endif /* __CONFIGS_PX4FMU_SRC_PX4FMU_INTERNAL_H */ - diff --git a/nuttx/configs/px4fmu/src/up_leds.c b/nuttx/configs/px4fmu/src/up_leds.c deleted file mode 100644 index f7b76aa58..000000000 --- a/nuttx/configs/px4fmu/src/up_leds.c +++ /dev/null @@ -1,127 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/src/up_leds.c - * arch/arm/src/board/up_leds.c - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX 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 - -#include "chip.h" -#include "up_arch.h" -#include "up_internal.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/* Enables debug output from this file (needs CONFIG_DEBUG with - * CONFIG_DEBUG_VERBOSE too) - */ - -#undef LED_DEBUG /* Define to enable debug */ - -#ifdef LED_DEBUG -# define leddbg lldbg -# define ledvdbg llvdbg -#else -# define leddbg(x...) -# define ledvdbg(x...) -#endif - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: up_ledinit - ****************************************************************************/ - -#ifdef CONFIG_ARCH_LEDS -void up_ledinit(void) -{ - /* Configure LED1-2 GPIOs for output */ - - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); -} - -/**************************************************************************** - * Name: up_ledon - ****************************************************************************/ - -void up_ledon(int led) -{ - if (led == 0) - { - /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); - } - if (led == 1) - { - /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED2, false); - } -} - -/**************************************************************************** - * Name: up_ledoff - ****************************************************************************/ - -void up_ledoff(int led) -{ - if (led == 0) - { - /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); - } - if (led == 1) - { - /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED2, true); - } -} - -#endif /* CONFIG_ARCH_LEDS */ -- cgit v1.2.3 From 7034acd07ead3e80bb0f767ae73ffa6fafcd375e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 11:01:05 +0100 Subject: Changed to UART5 for console --- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 91cd0366d..1c0b325b9 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=n +CONFIG_UART5_SERIAL_CONSOLE=y CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 -- cgit v1.2.3 From 472010b10b50436d8c40d94a7d9a888c1fe06858 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 12:59:14 +0100 Subject: Extended GPS struct with velocity vector --- apps/gps/ubx.c | 4 ++++ apps/uORB/topics/vehicle_gps_position.h | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index f676ffdfb..03ae622a1 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -479,6 +479,10 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { ubx_gps->vel = (uint16_t)packet->speed; + ubx_gps->vel_n = packet->velN / 100.0f; + ubx_gps->vel_e = packet->velE / 100.0f; + ubx_gps->vel_d = packet->velD / 100.0f; + ubx_gps->vel_ned_valid = true; ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f); ubx_gps->timestamp = hrt_absolute_time(); diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index 60d01a831..8e55ef148 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -66,6 +66,9 @@ struct vehicle_gps_position_s uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ + float vel_n; /**< GPS ground speed in m/s */ + float vel_e; /**< GPS ground speed in m/s */ + float vel_d; /**< GPS ground speed in m/s */ uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ @@ -77,6 +80,9 @@ struct vehicle_gps_position_s uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + /* flags */ + float vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + }; /** -- cgit v1.2.3 From 939fc83c4ad0bf299db35b7ec0c44dee47f3d033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 15:44:45 +0100 Subject: Fix compile warnings --- apps/systemlib/geo/geo.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 789368fbd..3709feb15 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -49,19 +49,19 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; + double lat_now_rad = lat_now / 180.0d * M_PI; + double lon_now_rad = lon_now / 180.0d * M_PI; + double lat_next_rad = lat_next / 180.0d * M_PI; + double lon_next_rad = lon_next / 180.0d * M_PI; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - const double radius_earth = 6371000.0; + const double radius_earth = 6371000.0d; return radius_earth * c; } -- cgit v1.2.3 From 8dcde7f8cd72e73ced0ea534a84257ef43210ab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 15:49:01 +0100 Subject: prevent double-precision promotion where its not required --- apps/systemlib/pid/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 7e277cdc7..0358caa25 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -155,8 +155,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabs(i) > pid->intmax) { + if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; -- cgit v1.2.3 From 0ddfd7c75c1f692fe83fcc88f832b42e2b04f0af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 16:31:21 +0100 Subject: New param interface for microSD and EEPROM --- apps/commander/commander.c | 39 +++----------------- apps/mavlink/mavlink_parameters.c | 62 +------------------------------ apps/systemcmds/param/param.c | 51 +++++++++++++++++-------- apps/systemlib/param/param.c | 78 +++++++++++++++++++++++++++++++++++++++ apps/systemlib/param/param.h | 19 ++++++++++ 5 files changed, 141 insertions(+), 108 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e00dc13d2..f2d92dc11 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -268,33 +268,6 @@ void tune_confirm() { ioctl(buzzer, TONE_SET_ALARM, 3); } -static const char *parameter_file = "/eeprom/parameters"; - -static int pm_save_eeprom(bool only_unsaved) -{ - /* delete the file in case it exists */ - unlink(parameter_file); - - /* create the file */ - int fd = open(parameter_file, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("opening '%s' for writing failed", parameter_file); - return -1; - } - - int result = param_export(fd, only_unsaved); - close(fd); - - if (result != 0) { - unlink(parameter_file); - warn("error exporting parameters to '%s'", parameter_file); - return -2; - } - - return 0; -} - void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ @@ -496,9 +469,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); + int save_ret = param_save_default(); if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); + warn("WARNING: auto-save of params to storage failed"); } printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -616,9 +589,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(fd); /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); + int save_ret = param_save_default(); if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); + warn("WARNING: auto-save of params to storage failed"); } // char buf[50]; @@ -736,9 +709,9 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(fd); /* auto-save to EEPROM */ - int save_ret = pm_save_eeprom(false); + int save_ret = param_save_default(); if(save_ret != 0) { - warn("WARNING: auto-save of params to EEPROM failed"); + warn("WARNING: auto-save of params to storage failed"); } //char buf[50]; diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index f41889535..6d434ed3d 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -172,64 +172,6 @@ int mavlink_pm_send_param(param_t param) return ret; } -static const char *mavlink_parameter_file = "/eeprom/parameters"; - -/** - * @return 0 on success, -1 if device open failed, -2 if writing parameters failed - */ -static int mavlink_pm_save_eeprom() -{ - /* delete the file in case it exists */ - unlink(mavlink_parameter_file); - - /* create the file */ - int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("opening '%s' for writing failed", mavlink_parameter_file); - return -1; - } - - int result = param_export(fd, false); - close(fd); - - if (result != 0) { - unlink(mavlink_parameter_file); - warn("error exporting parameters to '%s'", mavlink_parameter_file); - return -2; - } - - return 0; -} - -/** - * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed - */ -static int -mavlink_pm_load_eeprom() -{ - int fd = open(mavlink_parameter_file, O_RDONLY); - - if (fd < 0) { - /* no parameter file is OK, otherwise this is an error */ - if (errno != ENOENT) { - warn("open '%s' for reading failed", mavlink_parameter_file); - return -1; - } - return 1; - } - - int result = param_load(fd); - close(fd); - - if (result != 0) { - warn("error reading parameters from '%s'", mavlink_parameter_file); - return -2; - } - - return 0; -} - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { @@ -307,7 +249,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess if (((int)(cmd_mavlink.param1)) == 0) { /* read all parameters from EEPROM to RAM */ - int read_ret = mavlink_pm_load_eeprom(); + int read_ret = param_load_default(); if (read_ret == OK) { //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params"); @@ -327,7 +269,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } else if (((int)(cmd_mavlink.param1)) == 1) { /* write all parameters from RAM to EEPROM */ - int write_ret = mavlink_pm_save_eeprom(); + int write_ret = param_save_default(); if (write_ret == OK) { mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM"); result = MAV_RESULT_ACCEPTED; diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 68dbd822e..77f47b95d 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -56,36 +56,53 @@ __EXPORT int param_main(int argc, char *argv[]); -static void do_save(void); -static void do_load(void); -static void do_import(void); +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(void); static void do_show_print(void *arg, param_t param); -static const char *param_file_name = "/eeprom/parameters"; +static const char *param_file_name_default = "/fs/microsd/parameters"; int param_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "save")) - do_save(); + if (!strcmp(argv[1], "save")) { + if (argc >= 3) { + do_save(argv[2]); + } else { + do_save(param_file_name_default); + } + } - if (!strcmp(argv[1], "load")) - do_load(); + if (!strcmp(argv[1], "load")) { + if (argc >= 3) { + do_load(argv[2]); + } else { + do_load(param_file_name_default); + } + } - if (!strcmp(argv[1], "import")) - do_import(); + if (!strcmp(argv[1], "import")) { + if (argc >= 3) { + do_import(argv[2]); + } else { + do_import(param_file_name_default); + } + } - if (!strcmp(argv[1], "show")) + if (!strcmp(argv[1], "show")) { do_show(); + } + } errx(1, "expected a command, try 'load', 'import', 'show' or 'save'\n"); } static void -do_save(void) +do_save(const char* param_file_name) { /* delete the parameter file in case it exists */ unlink(param_file_name); @@ -108,7 +125,7 @@ do_save(void) } static void -do_load(void) +do_load(const char* param_file_name) { int fd = open(param_file_name, O_RDONLY); @@ -118,14 +135,18 @@ do_load(void) int result = param_load(fd); close(fd); - if (result < 0) + if (result < 0) { errx(1, "error importing from '%s'", param_file_name); + } else { + /* set default file name for next storage operation */ + param_set_default_file(param_file_name); + } exit(0); } static void -do_import(void) +do_import(const char* param_file_name) { int fd = open(param_file_name, O_RDONLY); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 365bd3d19..c63e7ca8d 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -480,6 +481,83 @@ param_reset_all(void) param_notify_changes(); } +static char param_default_file_name[50] = "/eeprom/parameters"; + +int +param_set_default_file(const char* filename) +{ + if (filename) { + if (strlen(filename) < sizeof(param_default_file_name)) + { + strcpy(param_default_file_name, filename); + } else { + warnx("param file name too long"); + return 1; + } + return 0; + } else { + warnx("no valid param file name"); + return 1; + } + return 0; +} + +int +param_save_default(void) +{ + /* delete the file in case it exists */ + unlink(param_default_file_name); + + /* create the file */ + int fd = open(param_default_file_name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("opening '%s' for writing failed", param_default_file_name); + return -1; + } + + int result = param_export(fd, false); + /* should not be necessary, over-careful here */ + fsync(fd); + close(fd); + + if (result != 0) { + unlink(param_default_file_name); + warn("error exporting parameters to '%s'", param_default_file_name); + return -2; + } + + return 0; +} + +/** + * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed + */ +int +param_load_default(void) +{ + int fd = open(param_default_file_name, O_RDONLY); + + if (fd < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + warn("open '%s' for reading failed", param_default_file_name); + return -1; + } + return 1; + } + + int result = param_load(fd); + close(fd); + + if (result != 0) { + warn("error reading parameters from '%s'", param_default_file_name); + return -2; + } + + return 0; +} + int param_export(int fd, bool only_unsaved) { diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index 41e268db0..64bb77834 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -234,6 +234,25 @@ __EXPORT int param_load(int fd); */ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); +/** + * Export parameters to the default file name. + * + * + * @param + */ +__EXPORT int param_save_default(void); + +/** + * Set the default parameter file name. + */ +__EXPORT int param_set_default_file(const char* filename); + +/** + * Import parameters from the default file name. + */ +__EXPORT int param_load_default(void); + + /* * Macros creating static parameter definitions. * -- cgit v1.2.3 From 8aed355a3fd357461bc91220df6b8fff86aa88ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 18:07:10 +0100 Subject: Reverted to IO compatible config --- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 1c0b325b9..91cd0366d 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=n +CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=y +CONFIG_UART5_SERIAL_CONSOLE=n CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 -- cgit v1.2.3 From 3c987d63680d153e4d954ad6249d24e5448e2204 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Oct 2012 18:50:00 +0100 Subject: Casting and fix default param path --- apps/mavlink/mavlink_receiver.c | 9 ++++----- apps/systemcmds/param/param.c | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index cfff0f469..826eb5625 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -253,14 +253,13 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; + offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ + if (quad_motors_setpoint.thrust[mavlink_system.sysid] == 0) { ml_armed = false; - } offboard_control_sp.armed = ml_armed; diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 77f47b95d..9cb280933 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -62,7 +62,7 @@ static void do_import(const char* param_file_name); static void do_show(void); static void do_show_print(void *arg, param_t param); -static const char *param_file_name_default = "/fs/microsd/parameters"; +static const char *param_file_name_default = "/eeprom/parameters"; int param_main(int argc, char *argv[]) -- cgit v1.2.3 From 8bfceef89cc1fd2422863a99d99039d18a1301bc Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 31 Oct 2012 12:59:24 -0700 Subject: Remove the arbitrary limit on the path to the default parameter file. Add a verb to the param command to set the default parameter file. --- apps/systemcmds/param/param.c | 21 ++++++++++++++------- apps/systemlib/param/param.c | 42 +++++++++++++++++++++--------------------- apps/systemlib/param/param.h | 29 ++++++++++++++++++++++------- 3 files changed, 57 insertions(+), 35 deletions(-) diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 9cb280933..92313e45a 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -62,8 +62,6 @@ static void do_import(const char* param_file_name); static void do_show(void); static void do_show_print(void *arg, param_t param); -static const char *param_file_name_default = "/eeprom/parameters"; - int param_main(int argc, char *argv[]) { @@ -72,7 +70,7 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_save(argv[2]); } else { - do_save(param_file_name_default); + do_save(param_get_default_file()); } } @@ -80,7 +78,7 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_load(argv[2]); } else { - do_load(param_file_name_default); + do_load(param_get_default_file()); } } @@ -88,17 +86,26 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_import(argv[2]); } else { - do_import(param_file_name_default); + 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 file %s", param_get_default_file()); + } + if (!strcmp(argv[1], "show")) { do_show(); } } - - errx(1, "expected a command, try 'load', 'import', 'show' or 'save'\n"); + + errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'"); } static void diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index c63e7ca8d..9a00c91a5 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -481,38 +481,38 @@ param_reset_all(void) param_notify_changes(); } -static char param_default_file_name[50] = "/eeprom/parameters"; +static const char *param_default_file = "/eeprom/parameters"; +static char *param_user_file; int param_set_default_file(const char* filename) { - if (filename) { - if (strlen(filename) < sizeof(param_default_file_name)) - { - strcpy(param_default_file_name, filename); - } else { - warnx("param file name too long"); - return 1; - } - return 0; - } else { - warnx("no valid param file name"); - return 1; + if (param_user_file != NULL) { + free(param_user_file); + param_user_file = NULL; } + if (filename) + param_user_file = strdup(filename); return 0; } +const char * +param_get_default_file(void) +{ + return (param_user_file != NULL) ? param_user_file : param_default_file; +} + int param_save_default(void) { /* delete the file in case it exists */ - unlink(param_default_file_name); + unlink(param_get_default_file()); /* create the file */ - int fd = open(param_default_file_name, O_WRONLY | O_CREAT | O_EXCL); + int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { - warn("opening '%s' for writing failed", param_default_file_name); + warn("opening '%s' for writing failed", param_get_default_file()); return -1; } @@ -522,8 +522,8 @@ param_save_default(void) close(fd); if (result != 0) { - unlink(param_default_file_name); - warn("error exporting parameters to '%s'", param_default_file_name); + unlink(param_get_default_file()); + warn("error exporting parameters to '%s'", param_get_default_file()); return -2; } @@ -536,12 +536,12 @@ param_save_default(void) int param_load_default(void) { - int fd = open(param_default_file_name, O_RDONLY); + int fd = open(param_get_default_file(), O_RDONLY); if (fd < 0) { /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { - warn("open '%s' for reading failed", param_default_file_name); + warn("open '%s' for reading failed", param_get_default_file()); return -1; } return 1; @@ -551,7 +551,7 @@ param_load_default(void) close(fd); if (result != 0) { - warn("error reading parameters from '%s'", param_default_file_name); + warn("error reading parameters from '%s'", param_get_default_file()); return -2; } diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index 64bb77834..6fa73b5a4 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -235,24 +235,39 @@ __EXPORT int param_load(int fd); __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); /** - * Export parameters to the default file name. + * Set the default parameter file name. * + * @param filename Path to the default parameter file. The file is not require to + * exist. + * @return Zero on success. + */ +__EXPORT int param_set_default_file(const char* filename); + +/** + * Get the default parameter file name. * - * @param + * @return The path to the current default parameter file; either as + * a result of a call to param_set_default_file, or the + * built-in default. */ -__EXPORT int param_save_default(void); +__EXPORT const char *param_get_default_file(void); /** - * Set the default parameter file name. + * Save parameters to the default file. + * + * This function saves all parameters with non-default values. + * + * @return Zero on success. */ -__EXPORT int param_set_default_file(const char* filename); +__EXPORT int param_save_default(void); /** - * Import parameters from the default file name. + * Load parameters from the default parameter file. + * + * @return Zero on success. */ __EXPORT int param_load_default(void); - /* * Macros creating static parameter definitions. * -- cgit v1.2.3