diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 65 |
1 files changed, 64 insertions, 1 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 392bc9f0a..dda1b825c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -119,11 +119,18 @@ public: /** * Initialize the PX4IO class. * - * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); /** + * Detect if a PX4IO is connected. + * + * Only validate if there is a PX4IO to talk to. + */ + virtual int detect(); + + /** * IO Control handler. * * Handle all IOCTL calls to the PX4IO file descriptor. @@ -476,6 +483,29 @@ PX4IO::~PX4IO() } int +PX4IO::detect() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + + return 0; +} + +int PX4IO::init() { int ret; @@ -1894,6 +1924,7 @@ start(int argc, char *argv[]) if (OK != g_dev->init()) { delete g_dev; + g_dev = nullptr; errx(1, "driver init failed"); } @@ -1921,6 +1952,35 @@ start(int argc, char *argv[]) } void +detect(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(0, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->detect()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver detect did not succeed"); + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(0); +} + +void bind(int argc, char *argv[]) { int pulses; @@ -2079,6 +2139,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); + if (!strcmp(argv[1], "detect")) + detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { |