diff --git a/src/drivers/boards/px4fmu-v5/px4fmu_init.c b/src/drivers/boards/px4fmu-v5/px4fmu_init.c index 74e057a373..74a2ebfe54 100644 --- a/src/drivers/boards/px4fmu-v5/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v5/px4fmu_init.c @@ -47,6 +47,7 @@ #include #include +#include #include #include @@ -89,22 +90,6 @@ /* Configuration ************************************************************/ -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) syslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message syslog -# 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 @@ -297,7 +282,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { - message("DMA alloc FAILED"); + PX4_ERR("DMA alloc FAILED"); } /* configure CPU load estimation */ @@ -359,7 +344,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (hadCrash == OK) { - message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \ + PX4_ERR("[boot] There is a hard fault logged. Hold down the SPACE BAR," \ " while booting to halt the system!\n"); /* Yes. So add one to the boot count - this will be reset after a successful @@ -381,7 +366,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false); - message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", + PX4_ERR("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n", reboots, (bytesWaiting == 0 ? "" : " Due to Key Press\n")); @@ -434,7 +419,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) break; } // Inner Switch - message("\nEnter B - Continue booting\n" \ + PX4_ERR("\nEnter B - Continue booting\n" \ "Enter C - Clear the fault log\n" \ "Enter D - Dump fault log\n\n?>"); fflush(stdout); @@ -471,6 +456,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif #ifdef CONFIG_MMCSD + ret = stm32_sdio_initialize(); if (ret != OK) { diff --git a/src/drivers/boards/px4fmu-v5/px4fmu_sdio.c b/src/drivers/boards/px4fmu-v5/px4fmu_sdio.c index a60a4df6b9..e65e21b0ae 100644 --- a/src/drivers/boards/px4fmu-v5/px4fmu_sdio.c +++ b/src/drivers/boards/px4fmu-v5/px4fmu_sdio.c @@ -37,6 +37,7 @@ ****************************************************************************/ #include +#include #include #include @@ -58,23 +59,6 @@ * Pre-processor Definitions ****************************************************************************/ -/* Configuration ************************************************************/ -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) syslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message syslog -# else -# define message printf -# endif -#endif - /* Card detections requires card support and a card detection GPIO */ #define HAVE_NCD 1 @@ -157,7 +141,7 @@ int stm32_sdio_initialize(void) sdio_dev = sdio_initialize(SDIO_SLOTNO); if (!sdio_dev) { - message("[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + PX4_ERR("[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); return -ENODEV; } @@ -168,7 +152,7 @@ int stm32_sdio_initialize(void) ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); if (ret != OK) { - message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + PX4_ERR("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } diff --git a/src/drivers/boards/px4fmu-v5/px4fmu_spi.c b/src/drivers/boards/px4fmu-v5/px4fmu_spi.c index c67d46fd5c..2791b88ec9 100644 --- a/src/drivers/boards/px4fmu-v5/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v5/px4fmu_spi.c @@ -42,6 +42,7 @@ ************************************************************************************/ #include +#include #include #include @@ -66,20 +67,6 @@ /* Debug ********************************************************************/ -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) syslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message syslog -# else -# define message printf -# endif -#endif - /* Define CS GPIO array */ static const uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO; static const uint32_t spi2selects_gpio[] = PX4_MEMORY_BUS_CS_GPIO; @@ -151,7 +138,7 @@ __EXPORT int stm32_spi_bus_initialize(void) spi_sensors = stm32_spibus_initialize(PX4_SPI_BUS_SENSORS); if (!spi_sensors) { - message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_SENSORS); return -ENODEV; } @@ -170,7 +157,7 @@ __EXPORT int stm32_spi_bus_initialize(void) spi_memory = stm32_spibus_initialize(PX4_SPI_BUS_MEMORY); if (!spi_memory) { - message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_MEMORY); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_MEMORY); return -ENODEV; } @@ -190,7 +177,7 @@ __EXPORT int stm32_spi_bus_initialize(void) spi_baro = stm32_spibus_initialize(PX4_SPI_BUS_BARO); if (!spi_baro) { - message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_BARO); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_BARO); return -ENODEV; } @@ -210,7 +197,7 @@ __EXPORT int stm32_spi_bus_initialize(void) spi_ext = stm32_spibus_initialize(PX4_SPI_BUS_EXTERNAL1); if (!spi_ext) { - message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL1); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL1); return -ENODEV; } @@ -227,7 +214,7 @@ __EXPORT int stm32_spi_bus_initialize(void) spi_ext = stm32_spibus_initialize(PX4_SPI_BUS_EXTERNAL2); if (!spi_ext) { - message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL2); + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_BUS_EXTERNAL2); return -ENODEV; }