diff options
author | px4dev <px4@purgatory.org> | 2013-07-14 11:50:35 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-07-14 11:50:35 -0700 |
commit | f8f6a43feac372c310f3d2444a8533b09e201d6c (patch) | |
tree | 4403d34866e38d44ccf365647eb12dc12a24e6de /src/drivers/px4io/px4io.cpp | |
parent | 5350c2be09af7eb88233cb89f8c013974a586e53 (diff) | |
download | px4-firmware-f8f6a43feac372c310f3d2444a8533b09e201d6c.tar.gz px4-firmware-f8f6a43feac372c310f3d2444a8533b09e201d6c.tar.bz2 px4-firmware-f8f6a43feac372c310f3d2444a8533b09e201d6c.zip |
Use common, board-type-agnostic code to allocate the PX4IO interface.
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 66 |
1 files changed, 32 insertions, 34 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 904da84c4..9f84c0950 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1598,30 +1598,43 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { -void -start(int argc, char *argv[]) +device::Device * +get_interface() { - if (g_dev != nullptr) - errx(1, "already loaded"); + device::Device *interface = nullptr; - device::Device *interface; + /* try for a serial interface */ + if (PX4IO_serial_interface != nullptr) + interface = PX4IO_serial_interface(); + if (interface != nullptr) + goto got; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif + /* try for an I2C interface if we haven't got a serial one */ + if (PX4IO_i2c_interface != nullptr) + interface = PX4IO_i2c_interface(); + if (interface != nullptr) + goto got; - if (interface == nullptr) - errx(1, "cannot alloc interface"); + errx(1, "cannot alloc interface"); - if (interface->init()) { +got: + if (interface->init() != OK) { delete interface; errx(1, "interface init failed"); } + return interface; +} + +void +start(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + /* create the driver - it will set g_dev */ (void)new PX4IO(interface); @@ -1741,27 +1754,12 @@ monitor(void) void if_test(unsigned mode) { - device::Device *interface; + device::Device *interface = get_interface(); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif + int result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; - if (interface == nullptr) - errx(1, "cannot alloc interface"); - - if (interface->init()) { - delete interface; - errx(1, "interface init failed"); - } else { - int result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; - errx(0, "test returned %d", result); - } + errx(0, "test returned %d", result); } } /* namespace */ |