aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/boards/aerocore/aerocore_init.c10
-rw-r--r--src/drivers/boards/aerocore/aerocore_spi.c2
-rw-r--r--src/drivers/boards/px4fmu-v1/px4fmu_init.c57
-rw-r--r--src/drivers/boards/px4fmu-v1/px4fmu_spi.c2
-rw-r--r--src/drivers/boards/px4fmu-v2/px4fmu2_init.c42
-rw-r--r--src/drivers/boards/px4fmu-v2/px4fmu_spi.c2
-rw-r--r--src/drivers/device/spi.h2
-rw-r--r--src/drivers/px4io/px4io_serial.cpp8
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp2
-rw-r--r--src/modules/px4iofirmware/px4io.c6
-rw-r--r--src/modules/px4iofirmware/sbus.c4
-rw-r--r--src/modules/sdlog2/sdlog2.c6
-rw-r--r--src/modules/systemlib/px4_macros.h96
-rw-r--r--src/systemcmds/esc_calib/esc_calib.c3
-rw-r--r--src/systemcmds/mtd/24xxxx_mtd.c2
-rw-r--r--src/systemcmds/mtd/mtd.c4
-rw-r--r--src/systemcmds/pwm/pwm.c4
-rw-r--r--src/systemcmds/tests/test_adc.c2
-rw-r--r--src/systemcmds/tests/test_hrt.c2
-rw-r--r--src/systemcmds/tests/test_jig_voltages.c2
-rw-r--r--src/systemcmds/tests/test_sensors.c2
-rw-r--r--src/systemcmds/tests/test_servo.c2
-rw-r--r--src/systemcmds/tests/tests_main.c2
23 files changed, 154 insertions, 110 deletions
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
index 1ce235da8..e370fa983 100644
--- a/src/drivers/boards/aerocore/aerocore_init.c
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -53,11 +53,11 @@
#include <errno.h>
#include <nuttx/arch.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
-#include <nuttx/gran.h>
+#include <nuttx/mm/gran.h>
#include <stm32.h>
#include "board_config.h"
@@ -191,7 +191,7 @@ stm32_boardinitialize(void)
stm32_spiinitialize();
/* configure LEDs */
- up_ledinit();
+ board_led_initialize();
}
/****************************************************************************
@@ -264,7 +264,7 @@ __EXPORT int nsh_archinitialize(void)
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
- up_ledon(LED_AMBER);
+ board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default: 1MHz, 8 bits, Mode 3 */
@@ -281,7 +281,7 @@ __EXPORT int nsh_archinitialize(void)
spi4 = up_spiinitialize(4);
if (!spi4) {
message("[boot] FAILED to initialize SPI port 4\n");
- up_ledon(LED_AMBER);
+ board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default: ~10MHz, 8 bits, Mode 3 */
diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c
index e329bd9d1..8577d4938 100644
--- a/src/drivers/boards/aerocore/aerocore_spi.c
+++ b/src/drivers/boards/aerocore/aerocore_spi.c
@@ -47,7 +47,7 @@
#include <stdbool.h>
#include <debug.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <arch/board/board.h>
#include <up_arch.h>
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 293021f8b..0e331a139 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -53,7 +53,7 @@
#include <errno.h>
#include <nuttx/arch.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
@@ -77,33 +77,6 @@
/* Debug ********************************************************************/
-#ifdef CONFIG_CPP_HAVE_VARARGS
-# ifdef CONFIG_DEBUG
-# define message(...) lowsyslog(__VA_ARGS__)
-# else
-# define message(...) printf(__VA_ARGS__)
-# endif
-#else
-# ifdef CONFIG_DEBUG
-# define message lowsyslog
-# else
-# define message printf
-# endif
-#endif
-
-/*
- * Ideally we'd be able to get these from up_internal.h,
- * but since we want to be able to disable the NuttX use
- * of leds for system indication at will and there is no
- * separate switch, we need to build independent of the
- * CONFIG_ARCH_LEDS configuration switch.
- */
-__BEGIN_DECLS
-extern void led_init(void);
-extern void led_on(int led);
-extern void led_off(int led);
-__END_DECLS
-
/****************************************************************************
* Protected Functions
****************************************************************************/
@@ -127,8 +100,8 @@ __EXPORT void stm32_boardinitialize(void)
/* configure SPI interfaces */
stm32_spiinitialize();
- /* configure LEDs (empty call to NuttX' ledinit) */
- up_ledinit();
+ /* configure LEDs */
+ board_led_initialize();
}
/****************************************************************************
@@ -202,8 +175,8 @@ __EXPORT int nsh_archinitialize(void)
spi1 = up_spiinitialize(1);
if (!spi1) {
- message("[boot] FAILED to initialize SPI port 1\r\n");
- up_ledon(LED_AMBER);
+ syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\r\n");
+ board_led_on(LED_AMBER);
return -ENODEV;
}
@@ -216,7 +189,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
- message("[boot] Successfully initialized SPI port 1\r\n");
+ syslog(LOG_INFO, "[boot] Successfully initialized SPI port 1\r\n");
/*
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
@@ -232,10 +205,10 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
- message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
+ syslog(LOG_INFO, "[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
#else
spi2 = NULL;
- message("[boot] Enabling IN12/13 instead of SPI2\n");
+ syslog(LOG_INFO, "[boot] Enabling IN12/13 instead of SPI2\n");
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
@@ -243,27 +216,27 @@ __EXPORT int nsh_archinitialize(void)
/* Get the SPI port for the microSD slot */
- message("[boot] Initializing SPI port 3\n");
+ syslog(LOG_INFO, "[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
- message("[boot] FAILED to initialize SPI port 3\n");
- up_ledon(LED_AMBER);
+ syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 3\n");
+ board_led_on(LED_AMBER);
return -ENODEV;
}
- message("[boot] Successfully initialized SPI port 3\n");
+ syslog(LOG_INFO, "[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) {
- message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
- up_ledon(LED_AMBER);
+ syslog(LOG_ERR, "[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+ board_led_on(LED_AMBER);
return -ENODEV;
}
- message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+ syslog(LOG_INFO, "[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
return OK;
}
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index 17e6862f7..0a39f22db 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -47,7 +47,7 @@
#include <stdbool.h>
#include <debug.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <arch/board/board.h>
#include "up_arch.h"
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 9b25c574a..ce1fa9f4e 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -53,12 +53,12 @@
#include <errno.h>
#include <nuttx/arch.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <nuttx/i2c.h>
#include <nuttx/sdio.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
-#include <nuttx/gran.h>
+#include <nuttx/mm/gran.h>
#include <stm32.h>
#include "board_config.h"
@@ -94,19 +94,6 @@
# endif
#endif
-/*
- * Ideally we'd be able to get these from up_internal.h,
- * but since we want to be able to disable the NuttX use
- * of leds for system indication at will and there is no
- * separate switch, we need to build independent of the
- * CONFIG_ARCH_LEDS configuration switch.
- */
-__BEGIN_DECLS
-extern void led_init(void);
-extern void led_on(int led);
-extern void led_off(int led);
-__END_DECLS
-
/****************************************************************************
* Protected Functions
****************************************************************************/
@@ -116,6 +103,8 @@ __END_DECLS
# error microSD DMA support requires CONFIG_GRAN
# endif
+#ifdef CONFIG_FAT_DMAMEMORY
+
static GRAN_HANDLE dma_allocator;
/*
@@ -130,6 +119,7 @@ static GRAN_HANDLE dma_allocator;
*/
static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
static perf_counter_t g_dma_perf;
+#endif
static void
dma_alloc_init(void)
@@ -139,7 +129,7 @@ dma_alloc_init(void)
7, /* 128B granule - must be > alignment (XXX bug?) */
6); /* 64B alignment */
if (dma_allocator == NULL) {
- message("[boot] DMA allocator setup FAILED");
+ syslog(LOG_ERR, "[boot] DMA allocator setup FAILED");
} else {
g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
}
@@ -192,7 +182,7 @@ stm32_boardinitialize(void)
stm32_spiinitialize();
/* configure LEDs */
- up_ledinit();
+ board_led_initialize();
}
/****************************************************************************
@@ -281,8 +271,8 @@ __EXPORT int nsh_archinitialize(void)
spi1 = up_spiinitialize(1);
if (!spi1) {
- message("[boot] FAILED to initialize SPI port 1\n");
- up_ledon(LED_AMBER);
+ syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n");
+ board_led_on(LED_AMBER);
return -ENODEV;
}
@@ -296,15 +286,15 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
- message("[boot] Initialized SPI port 1 (SENSORS)\n");
+ syslog(LOG_INFO, "[boot] Initialized SPI port 1 (SENSORS)\n");
/* Get the SPI port for the FRAM */
spi2 = up_spiinitialize(2);
if (!spi2) {
- message("[boot] FAILED to initialize SPI port 2\n");
- up_ledon(LED_AMBER);
+ syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n");
+ board_led_on(LED_AMBER);
return -ENODEV;
}
@@ -317,7 +307,7 @@ __EXPORT int nsh_archinitialize(void)
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
- message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
+ syslog(LOG_INFO, "[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
spi4 = up_spiinitialize(4);
@@ -335,7 +325,7 @@ __EXPORT int nsh_archinitialize(void)
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
- message("[boot] Failed to initialize SDIO slot %d\n",
+ syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
@@ -343,14 +333,14 @@ __EXPORT int nsh_archinitialize(void)
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
- message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+ syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
- message("[boot] Initialized SDIO\n");
+ syslog(LOG_INFO, "[boot] Initialized SDIO\n");
#endif
return OK;
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index 27f193513..b98cd999b 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -47,7 +47,7 @@
#include <stdbool.h>
#include <debug.h>
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
#include <arch/board/board.h>
#include <up_arch.h>
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 1d9837689..95b06cd1a 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -42,7 +42,7 @@
#include "device.h"
-#include <nuttx/spi.h>
+#include <nuttx/spi/spi.h>
namespace device __EXPORT
{
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index d227e15d5..928e2dab8 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -292,7 +292,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
case 1: /* XXX magic number - test operation */
switch (arg) {
case 0:
- lowsyslog("test 0\n");
+ lowsyslog(LOG_INFO, "test 0\n");
/* kill DMA, this is a PIO test */
stm32_dmastop(_tx_dma);
@@ -316,7 +316,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
fails++;
if (count >= 5000) {
- lowsyslog("==== test 1 : %u failures ====\n", fails);
+ lowsyslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
perf_print_counter(_pc_txns);
perf_print_counter(_pc_dmasetup);
perf_print_counter(_pc_retries);
@@ -333,7 +333,7 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
return 0;
}
case 2:
- lowsyslog("test 2\n");
+ lowsyslog(LOG_INFO, "test 2\n");
return 0;
}
default:
@@ -555,7 +555,7 @@ PX4IO_serial::_wait_complete()
}
/* we might? see this for EINTR */
- lowsyslog("unexpected ret %d/%d\n", ret, errno);
+ lowsyslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno);
}
/* reset DMA status */
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 8f523b390..13796935b 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -672,7 +672,7 @@ ToneAlarm::next_note()
// tune looks bad (unexpected EOF, bad character, etc.)
tune_error:
- lowsyslog("tune error\n");
+ lowsyslog(LOG_ERR, "tune error\n");
_repeat = false; // don't loop on error
// stop (and potentially restart) the tune
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 14ee9cb40..c31342262 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -247,7 +247,7 @@ user_start(int argc, char *argv[])
#endif
/* print some startup info */
- lowsyslog("\nPX4IO: starting\n");
+ lowsyslog(LOG_INFO, "\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
@@ -292,7 +292,7 @@ user_start(int argc, char *argv[])
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
- lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
+ lowsyslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
@@ -312,7 +312,7 @@ user_start(int argc, char *argv[])
*/
if (minfo.mxordblk < 600) {
- lowsyslog("ERR: not enough MEM");
+ lowsyslog(LOG_ERR, "ERR: not enough MEM");
bool phase = false;
while (true) {
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index d76ec55f0..fee9d656d 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -112,10 +112,8 @@ sbus_init(const char *device)
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
- debug("S.Bus: ready");
-
} else {
- debug("S.Bus: open failed");
+ dbg("S.Bus: open failed");
}
return sbus_fd;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0a8564da6..fd7281f90 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -54,6 +54,7 @@
#include <string.h>
#include <ctype.h>
#include <systemlib/err.h>
+#include <systemlib/px4_macros.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <math.h>
@@ -1048,6 +1049,11 @@ int sdlog2_thread_main(int argc, char *argv[])
int encoders_sub;
} subs;
+ /* Make sure we have enough file descriptors to support this many subscriptions
+ * taking into account stdin,stdout and std err
+ */
+ CCASSERT((sizeof(subs) / sizeof (int)) < CONFIG_NFILE_DESCRIPTORS-3);
+
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
diff --git a/src/modules/systemlib/px4_macros.h b/src/modules/systemlib/px4_macros.h
new file mode 100644
index 000000000..32eaf266d
--- /dev/null
+++ b/src/modules/systemlib/px4_macros.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 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 px4_macros.h
+ *
+ * A set of useful macros for enhanced runtime and compile time
+ * error detection and warning suppression.
+ *
+ * Define NO_BLOAT to reduce bloat from file name inclusion.
+ *
+ * The arraySize() will compute the size of an array regardless
+ * it's type
+ *
+ * INVALID_CASE(c) should be used is case statements to ferret out
+ * unintended behavior
+ *
+ * UNUSED(var) will suppress compile time warnings of unused
+ * variables
+ *
+ * CCASSERT(predicate) Will generate a compile time error it the
+ * predicate is false
+ */
+
+#ifndef _PX4_MACROS_H
+#define _PX4_MACROS_H
+
+
+#if !defined(arraySize)
+#define arraySize(a) (sizeof((a))/sizeof((a[0])))
+#endif
+
+#if !defined(NO_BLOAT)
+#if defined(__BASE_FILE__)
+#define _FILE_NAME_ __BASE_FILE__
+#else
+#define _FILE_NAME_ __FILE__
+#endif
+#else
+#define _FILE_NAME_ ""
+#endif
+
+#if !defined(INVALID_CASE)
+#define INVALID_CASE(c) printf("Invalid Case %d, %s:%d",(c),__BASE_FILE__,__LINE__) /* todo use PANIC */
+#endif
+
+#if !defined(UNUSED)
+#define UNUSED(var) (void)(var)
+#endif
+
+#if !defined(CAT)
+#if !defined(_CAT)
+#define _CAT(a, b) a ## b
+#endif
+#define CAT(a, b) _CAT(a, b)
+#endif
+
+#if !defined(CCASSERT)
+#define CCASSERT(predicate) _x_CCASSERT_LINE(predicate, __LINE__)
+#if !defined(_x_CCASSERT_LINE)
+#define _x_CCASSERT_LINE(predicate, line) typedef char CAT(constraint_violated_on_line_,line)[2*((predicate)!=0)-1];
+#endif
+#endif
+
+
+#endif /* _PX4_MACROS_H */
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 7d80af307..c464ab84d 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -51,9 +51,6 @@
#include <sys/ioctl.h>
#include <sys/stat.h>
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index f85ed8e2d..d0407f5aa 100644
--- a/src/systemcmds/mtd/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
@@ -64,7 +64,7 @@
#include <nuttx/kmalloc.h>
#include <nuttx/fs/ioctl.h>
#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
+#include <nuttx/mtd/mtd.h>
#include "systemlib/perf_counter.h"
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index a925cdd40..16bfb294f 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -51,8 +51,8 @@
#include <sys/ioctl.h>
#include <sys/stat.h>
-#include <nuttx/spi.h>
-#include <nuttx/mtd.h>
+#include <nuttx/spi/spi.h>
+#include <nuttx/mtd/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index eeba89fa8..4bc1a727b 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -46,13 +46,9 @@
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
-#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
-#include <nuttx/i2c.h>
-#include <nuttx/mtd.h>
-#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index 03391b851..82f39439d 100644
--- a/src/systemcmds/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
@@ -48,8 +48,6 @@
#include <errno.h>
#include <debug.h>
-#include <nuttx/spi.h>
-
#include "tests.h"
#include <nuttx/analog/adc.h>
diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index 5690997a9..52b703bd2 100644
--- a/src/systemcmds/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
@@ -54,8 +54,6 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
-#include <nuttx/spi.h>
-
#include "tests.h"
/****************************************************************************
diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c
index 10c93b264..073474620 100644
--- a/src/systemcmds/tests/test_jig_voltages.c
+++ b/src/systemcmds/tests/test_jig_voltages.c
@@ -47,8 +47,6 @@
#include <errno.h>
#include <debug.h>
-#include <nuttx/spi.h>
-
#include "tests.h"
#include <nuttx/analog/adc.h>
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index e005bf9c1..d87551bd9 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -53,8 +53,6 @@
#include <arch/board/board.h>
-#include <nuttx/spi.h>
-
#include "tests.h"
#include <drivers/drv_gyro.h>
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index 9c6951ca2..9545e7dc9 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -53,8 +53,6 @@
#include <drivers/drv_pwm_output.h>
#include <systemlib/err.h>
-#include <nuttx/spi.h>
-
#include "tests.h"
int test_servo(int argc, char *argv[])
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 0f56704e6..16fa3469f 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -52,8 +52,6 @@
#include <arch/board/board.h>
-#include <nuttx/spi.h>
-
#include <systemlib/perf_counter.h>
#include "tests.h"