aboutsummaryrefslogtreecommitdiff
path: root/nuttx
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2012-10-31 21:07:01 +0100
committerThomas Gubler <thomasgubler@gmail.com>2012-10-31 21:07:01 +0100
commit904efa8fa87c09a2e7f18f0821431e75eb13e67b (patch)
treee63d4b92a39994f5b396a55a74898221d61e795d /nuttx
parentee1e98babb3b46de0c64dbd0c5be3c678fc7c727 (diff)
parent8bfceef89cc1fd2422863a99d99039d18a1301bc (diff)
downloadpx4-firmware-904efa8fa87c09a2e7f18f0821431e75eb13e67b.tar.gz
px4-firmware-904efa8fa87c09a2e7f18f0821431e75eb13e67b.tar.bz2
px4-firmware-904efa8fa87c09a2e7f18f0821431e75eb13e67b.zip
Merge branch 'master' of https://github.com/PX4/Firmware into fw_control
Diffstat (limited to 'nuttx')
-rw-r--r--nuttx/configs/px4fmu/include/drv_eeprom.h60
-rw-r--r--nuttx/configs/px4fmu/include/drv_led.h51
-rw-r--r--nuttx/configs/px4fmu/nsh/appconfig2
-rw-r--r--nuttx/configs/px4fmu/src/Makefile5
-rw-r--r--nuttx/configs/px4fmu/src/drv_eeprom.c522
-rw-r--r--nuttx/configs/px4fmu/src/drv_led.c113
-rw-r--r--nuttx/configs/px4fmu/src/empty.c4
-rw-r--r--nuttx/configs/px4fmu/src/px4fmu-internal.h166
-rw-r--r--nuttx/configs/px4fmu/src/up_leds.c127
9 files changed, 7 insertions, 1043 deletions
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/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 <sys/ioctl.h>
-
-#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 67fe69a30..c696407ea 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
@@ -96,6 +95,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 13b26b84f..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 drv_eeprom.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_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 <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-#include <errno.h>
-#include <string.h>
-#include <stdio.h>
-#include <unistd.h>
-
-#include <nuttx/i2c.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_eeprom.h>
-
-/* 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;
-}
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 <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-#include "px4fmu-internal.h"
-
-#include <arch/board/drv_led.h>
-
-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 <gnutt@nuttx.org>
- *
- * 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 <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-/****************************************************************************************************
- * 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 <gnutt@nuttx.org>
- *
- * 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 <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#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 */