diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 5e408cd544..9fae9240d7 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -9,7 +9,7 @@ param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 param set CAL_MAG0_ID 196608 rgbled start -mavlink start +mavlink start -d /tmp/ttyS0 sensors start hil mode_pwm commander start diff --git a/src/drivers/blinkm/blinkm_posix.cpp b/src/drivers/blinkm/blinkm_posix.cpp index 5a139a710a..04d0e5bcb5 100644 --- a/src/drivers/blinkm/blinkm_posix.cpp +++ b/src/drivers/blinkm/blinkm_posix.cpp @@ -140,7 +140,7 @@ public: virtual int init(); virtual int probe(); virtual int setMode(int mode); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); static const char *const script_names[]; @@ -358,7 +358,7 @@ BlinkM::probe() } int -BlinkM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = ENOTTY; diff --git a/src/drivers/device/cdev_nuttx.cpp b/src/drivers/device/cdev.cpp similarity index 86% rename from src/drivers/device/cdev_nuttx.cpp rename to src/drivers/device/cdev.cpp index 6db0400373..3bc05b0c73 100644 --- a/src/drivers/device/cdev_nuttx.cpp +++ b/src/drivers/device/cdev.cpp @@ -60,13 +60,13 @@ static const unsigned pollset_increment = 0; * The standard NuttX operation dispatch table can't call C++ member functions * directly, so we have to bounce them through this dispatch table. */ -static int cdev_open(struct file *filp); -static int cdev_close(struct file *filp); -static ssize_t cdev_read(struct file *filp, char *buffer, size_t buflen); -static ssize_t cdev_write(struct file *filp, const char *buffer, size_t buflen); -static off_t cdev_seek(struct file *filp, off_t offset, int whence); -static int cdev_ioctl(struct file *filp, int cmd, unsigned long arg); -static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup); +static int cdev_open(file_t *filp); +static int cdev_close(file_t *filp); +static ssize_t cdev_read(file_t *filp, char *buffer, size_t buflen); +static ssize_t cdev_write(file_t *filp, const char *buffer, size_t buflen); +static off_t cdev_seek(file_t *filp, off_t offset, int whence); +static int cdev_ioctl(file_t *filp, int cmd, unsigned long arg); +static int cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup); /** * Character device indirection table. @@ -169,7 +169,7 @@ out: * Default implementations of the character device interface */ int -CDev::open(struct file *filp) +CDev::open(file_t *filp) { int ret = OK; @@ -192,13 +192,13 @@ CDev::open(struct file *filp) } int -CDev::open_first(struct file *filp) +CDev::open_first(file_t *filp) { return OK; } int -CDev::close(struct file *filp) +CDev::close(file_t *filp) { int ret = OK; @@ -222,31 +222,31 @@ CDev::close(struct file *filp) } int -CDev::close_last(struct file *filp) +CDev::close_last(file_t *filp) { return OK; } ssize_t -CDev::read(struct file *filp, char *buffer, size_t buflen) +CDev::read(file_t *filp, char *buffer, size_t buflen) { return -ENOSYS; } ssize_t -CDev::write(struct file *filp, const char *buffer, size_t buflen) +CDev::write(file_t *filp, const char *buffer, size_t buflen) { return -ENOSYS; } off_t -CDev::seek(struct file *filp, off_t offset, int whence) +CDev::seek(file_t *filp, off_t offset, int whence) { return -ENOSYS; } int -CDev::ioctl(struct file *filp, int cmd, unsigned long arg) +CDev::ioctl(file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -275,7 +275,7 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) } int -CDev::poll(struct file *filp, struct pollfd *fds, bool setup) +CDev::poll(file_t *filp, struct pollfd *fds, bool setup) { int ret = OK; @@ -347,7 +347,7 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) } pollevent_t -CDev::poll_state(struct file *filp) +CDev::poll_state(file_t *filp) { /* by default, no poll events to report */ return 0; @@ -389,7 +389,7 @@ CDev::remove_poll_waiter(struct pollfd *fds) } static int -cdev_open(struct file *filp) +cdev_open(file_t *filp) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -397,7 +397,7 @@ cdev_open(struct file *filp) } static int -cdev_close(struct file *filp) +cdev_close(file_t *filp) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -405,7 +405,7 @@ cdev_close(struct file *filp) } static ssize_t -cdev_read(struct file *filp, char *buffer, size_t buflen) +cdev_read(file_t *filp, char *buffer, size_t buflen) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -413,7 +413,7 @@ cdev_read(struct file *filp, char *buffer, size_t buflen) } static ssize_t -cdev_write(struct file *filp, const char *buffer, size_t buflen) +cdev_write(file_t *filp, const char *buffer, size_t buflen) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -421,7 +421,7 @@ cdev_write(struct file *filp, const char *buffer, size_t buflen) } static off_t -cdev_seek(struct file *filp, off_t offset, int whence) +cdev_seek(file_t *filp, off_t offset, int whence) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -429,7 +429,7 @@ cdev_seek(struct file *filp, off_t offset, int whence) } static int -cdev_ioctl(struct file *filp, int cmd, unsigned long arg) +cdev_ioctl(file_t *filp, int cmd, unsigned long arg) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -437,7 +437,7 @@ cdev_ioctl(struct file *filp, int cmd, unsigned long arg) } static int -cdev_poll(struct file *filp, struct pollfd *fds, bool setup) +cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup) { CDev *cdev = (CDev *)(filp->f_inode->i_private); diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 3d5737cee4..a3a0640d2d 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -44,6 +44,7 @@ * Includes here should only cover the needs of the framework definitions. */ #include +#include #include #include @@ -59,6 +60,8 @@ namespace device __EXPORT { +typedef struct file file_t; + /** * Fundamental base class for all device drivers. * @@ -275,7 +278,7 @@ public: * @param filp Pointer to the NuttX file structure. * @return OK if the open is allowed, -errno otherwise. */ - virtual int open(struct file *filp); + virtual int open(file_t *filp); /** * Handle a close of the device. @@ -286,7 +289,7 @@ public: * @param filp Pointer to the NuttX file structure. * @return OK if the close was successful, -errno otherwise. */ - virtual int close(struct file *filp); + virtual int close(file_t *filp); /** * Perform a read from the device. @@ -298,7 +301,7 @@ public: * @param buflen The number of bytes to be read. * @return The number of bytes read or -errno otherwise. */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(file_t *filp, char *buffer, size_t buflen); /** * Perform a write to the device. @@ -310,7 +313,7 @@ public: * @param buflen The number of bytes to be written. * @return The number of bytes written or -errno otherwise. */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual ssize_t write(file_t *filp, const char *buffer, size_t buflen); /** * Perform a logical seek operation on the device. @@ -322,7 +325,7 @@ public: * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. * @return The previous offset, or -errno otherwise. */ - virtual off_t seek(struct file *filp, off_t offset, int whence); + virtual off_t seek(file_t *filp, off_t offset, int whence); /** * Perform an ioctl operation on the device. @@ -336,7 +339,7 @@ public: * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(file_t *filp, int cmd, unsigned long arg); /** * Perform a poll setup/teardown operation. @@ -349,7 +352,7 @@ public: * it is being torn down. * @return OK on success, or -errno otherwise. */ - virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + virtual int poll(file_t *filp, struct pollfd *fds, bool setup); /** * Test whether the device is currently open. @@ -380,7 +383,7 @@ protected: * @param filp The file that's interested. * @return The current set of poll events. */ - virtual pollevent_t poll_state(struct file *filp); + virtual pollevent_t poll_state(file_t *filp); /** * Report new poll events. @@ -398,7 +401,7 @@ protected: * @param fds A poll waiter to notify. * @param events The event(s) to send to the waiter. */ - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); /** * Notification of the first open. @@ -411,7 +414,7 @@ protected: * @param filp Pointer to the NuttX file structure. * @return OK if the open should proceed, -errno otherwise. */ - virtual int open_first(struct file *filp); + virtual int open_first(file_t *filp); /** * Notification of the last close. @@ -424,7 +427,7 @@ protected: * @param filp Pointer to the NuttX file structure. * @return OK if the open should return OK, -errno otherwise. */ - virtual int close_last(struct file *filp); + virtual int close_last(file_t *filp); /** * Register a class device name, automatically adding device @@ -470,7 +473,7 @@ private: * * @return OK, or -errno on error. */ - int store_poll_waiter(struct pollfd *fds); + int store_poll_waiter(px4_pollfd_struct_t *fds); /** * Remove a poll waiter. diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device_posix.cpp similarity index 100% rename from src/drivers/device/device.cpp rename to src/drivers/device/device_posix.cpp diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 51e8e2083e..c04cfaafb3 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -221,7 +221,7 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) return ret; } -int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; @@ -231,11 +231,11 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) return 0; default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } -ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be @@ -246,7 +246,7 @@ ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) return ::read(_fd, buffer, buflen); } -ssize_t I2C::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) { if (simulate) { warnx ("2C SIM I2C::write"); diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h index e01e3c242d..c05100ae43 100644 --- a/src/drivers/device/i2c_posix.h +++ b/src/drivers/device/i2c_posix.h @@ -93,9 +93,9 @@ protected: virtual int init(); - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(file_t *handlep, char *buffer, size_t buflen); + virtual ssize_t write(file_t *handlep, const char *buffer, size_t buflen); + virtual int ioctl(file_t *handlep, int cmd, unsigned long arg); /** * Perform an I2C transaction to the device. diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index f7883568e9..22e7b15622 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -38,14 +38,15 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ device_nuttx.cpp \ - cdev_nuttx.cpp \ + cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ spi.cpp \ ringbuffer.cpp else -SRCS = vdev.cpp \ - device.cpp \ +SRCS = \ + device_posix.cpp \ + vdev.cpp \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0c1dc3abe9..699bc817b1 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -207,7 +207,7 @@ out: * Default implementations of the character device interface */ int -VDev::open(px4_dev_handle_t *handlep) +VDev::open(file_t *filep) { int ret = PX4_OK; @@ -219,7 +219,7 @@ VDev::open(px4_dev_handle_t *handlep) if (_open_count == 1) { /* first-open callback may decline the open */ - ret = open_first(handlep); + ret = open_first(filep); if (ret != PX4_OK) _open_count--; @@ -231,14 +231,14 @@ VDev::open(px4_dev_handle_t *handlep) } int -VDev::open_first(px4_dev_handle_t *handlep) +VDev::open_first(file_t *filep) { debug("VDev::open_first"); return PX4_OK; } int -VDev::close(px4_dev_handle_t *handlep) +VDev::close(file_t *filep) { debug("VDev::close"); int ret = PX4_OK; @@ -251,7 +251,7 @@ VDev::close(px4_dev_handle_t *handlep) /* callback cannot decline the close */ if (_open_count == 0) - ret = close_last(handlep); + ret = close_last(filep); } else { ret = -EBADF; @@ -263,34 +263,34 @@ VDev::close(px4_dev_handle_t *handlep) } int -VDev::close_last(px4_dev_handle_t *handlep) +VDev::close_last(file_t *filep) { debug("VDev::close_last"); return PX4_OK; } ssize_t -VDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +VDev::read(file_t *filep, char *buffer, size_t buflen) { debug("VDev::read"); return -ENOSYS; } ssize_t -VDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +VDev::write(file_t *filep, const char *buffer, size_t buflen) { debug("VDev::write"); return -ENOSYS; } off_t -VDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) +VDev::seek(file_t *filep, off_t offset, int whence) { return -ENOSYS; } int -VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +VDev::ioctl(file_t *filep, int cmd, unsigned long arg) { int ret = -ENOTTY; @@ -320,7 +320,7 @@ VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) } int -VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) +VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; debug("VDev::Poll %s", setup ? "setup" : "teardown"); @@ -335,8 +335,8 @@ VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * Save the file pointer in the pollfd for the subclass' * benefit. */ - fds->priv = (void *)handlep; - debug("VDev::poll: fds->priv = %p", handlep); + fds->priv = (void *)filep; + debug("VDev::poll: fds->priv = %p", filep); /* * Handle setup requests. @@ -349,7 +349,7 @@ VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * Check to see whether we should send a poll notification * immediately. */ - fds->revents |= fds->events & poll_state(handlep); + fds->revents |= fds->events & poll_state(filep); /* yes? post the notification */ if (fds->revents != 0) @@ -402,7 +402,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) } pollevent_t -VDev::poll_state(px4_dev_handle_t *handlep) +VDev::poll_state(file_t *filep) { debug("VDev::poll_notify"); /* by default, no poll events to report */ diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 6448b8aa98..3cf0a3f691 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -57,17 +57,14 @@ namespace device __EXPORT { -#define PX4_F_RDONLY 1 -#define PX4_F_WRONLY 2 - -struct px4_dev_handle_t { +struct file_t { int fd; int flags; void *priv; void *vdev; - px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {} - px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {} + file_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {} + file_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {} }; /** @@ -242,13 +239,13 @@ public: * * This is handled internally and should not normally be overridden. * - * @param handlep Pointer to the internal file structure. + * @param filep Pointer to the internal file structure. * @param fds Poll descriptor being waited on. * @param setup True if this is establishing a request, false if * it is being torn down. * @return OK on success, or -errno otherwise. */ - virtual int poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup); + virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); /** * Test whether the device is currently open. @@ -266,10 +263,10 @@ public: * This function is called for every open of the device. The default * implementation maintains _open_count and always returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open is allowed, -errno otherwise. */ - virtual int open(px4_dev_handle_t *handlep); + virtual int open(file_t *filep); /** * Handle a close of the device. @@ -277,46 +274,46 @@ public: * This function is called for every close of the device. The default * implementation maintains _open_count and returns OK as long as it is not zero. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the close was successful, -errno otherwise. */ - virtual int close(px4_dev_handle_t *handlep); + virtual int close(file_t *filep); /** * Perform a read from the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param buffer Pointer to the buffer into which data should be placed. * @param buflen The number of bytes to be read. * @return The number of bytes read or -errno otherwise. */ - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual ssize_t read(file_t *filep, char *buffer, size_t buflen); /** * Perform a write to the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param buffer Pointer to the buffer from which data should be read. * @param buflen The number of bytes to be written. * @return The number of bytes written or -errno otherwise. */ - virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t write(file_t *filep, const char *buffer, size_t buflen); /** * Perform a logical seek operation on the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param offset The new file position relative to whence. * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. * @return The previous offset, or -errno otherwise. */ - virtual off_t seek(px4_dev_handle_t *handlep, off_t offset, int whence); + virtual off_t seek(file_t *filep, off_t offset, int whence); /** * Perform an ioctl operation on the device. @@ -325,12 +322,12 @@ public: * returns -ENOTTY. Subclasses should call the default implementation * for any command they do not handle themselves. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param cmd The ioctl command value. * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(file_t *filep, int cmd, unsigned long arg); static VDev *getDev(const char *path); static void showDevices(void); @@ -352,10 +349,10 @@ protected: * * The default implementation returns no events. * - * @param handlep The file that's interested. + * @param filep The file that's interested. * @return The current set of poll events. */ - virtual pollevent_t poll_state(px4_dev_handle_t *handlep); + virtual pollevent_t poll_state(file_t *filep); /** * Report new poll events. @@ -383,10 +380,10 @@ protected: * * The default implementation returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open should proceed, -errno otherwise. */ - virtual int open_first(px4_dev_handle_t *handlep); + virtual int open_first(file_t *filep); /** * Notification of the last close. @@ -396,10 +393,10 @@ protected: * * The default implementation returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open should return OK, -errno otherwise. */ - virtual int close_last(px4_dev_handle_t *handlep); + virtual int close_last(file_t *filep); /** * Register a class device name, automatically adding device diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 58b445d607..e0e643ae2c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file vcdev_posix.cpp + * @file vdev_posix.cpp * * POSIX-like API for virtual character device */ @@ -47,6 +47,8 @@ #include #include +#define PX4_DEBUG(...) + using namespace device; extern "C" { @@ -74,7 +76,7 @@ static void *timer_handler(void *data) } #define PX4_MAX_FD 100 -static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; +static device::file_t *filemap[PX4_MAX_FD] = {}; int px4_errno; @@ -95,7 +97,7 @@ int px4_open(const char *path, int flags) else { for (i=0; i +#include +#include +#include #include #include @@ -60,11 +63,10 @@ #include #include #include -#include +#include +#include #include -#include - #include #include #include @@ -82,21 +84,25 @@ #include +#ifdef __PX4_NUTTX class HIL : public device::CDev +#else +class HIL : public device::VDev +#endif { public: enum Mode { MODE_2PWM, MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, MODE_NONE }; HIL(); virtual ~HIL(); - virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -131,7 +137,7 @@ private: uint8_t control_index, float &input); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); + int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); struct GPIOConfig { uint32_t input; @@ -146,7 +152,7 @@ private: void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); + int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg); }; @@ -158,7 +164,12 @@ HIL *g_hil; } // namespace HIL::HIL() : - CDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), +#ifdef __PX4_NUTTX + CDev( +#else + VDev( +#endif + "hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), @@ -188,7 +199,7 @@ HIL::~HIL() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -211,7 +222,11 @@ HIL::init() ASSERT(_task == -1); /* do regular cdev init */ +#ifdef __PX4_NUTTX ret = CDev::init(); +#else + ret = VDev::init(); +#endif if (ret != OK) return ret; @@ -228,11 +243,11 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = task_spawn_cmd("fmuhil", + _task = px4_task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1200, - (main_t)&HIL::task_main_trampoline, + (px4_main_t)&HIL::task_main_trampoline, nullptr); if (_task < 0) { @@ -273,22 +288,22 @@ HIL::set_mode(Mode mode) break; case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; case MODE_NONE: debug("MODE_NONE"); @@ -336,7 +351,7 @@ HIL::task_main() _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &dummy, ORB_PRIO_LOW); - pollfd fds[2]; + px4_pollfd_struct_t fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; @@ -383,7 +398,7 @@ HIL::task_main() } /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + int ret = px4_poll(&fds[0], 2, 1000); /* this would be bad... */ if (ret < 0) { @@ -410,7 +425,7 @@ HIL::task_main() /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - isfinite(outputs.output[i]) && + PX4_ISFINITE(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ @@ -439,8 +454,8 @@ HIL::task_main() } } - ::close(_t_actuators); - ::close(_t_armed); + px4_close(_t_actuators); + px4_close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); @@ -449,7 +464,6 @@ HIL::task_main() /* tell the dtor that we are exiting */ _task = -1; - _exit(0); } int @@ -465,7 +479,7 @@ HIL::control_callback(uintptr_t handle, } int -HIL::ioctl(file *filp, int cmd, unsigned long arg) +HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret; @@ -480,9 +494,9 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) switch(_mode) { case MODE_2PWM: case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: ret = HIL::pwm_ioctl(filp, cmd, arg); break; default: @@ -492,14 +506,19 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { +#ifdef __PX4_NUTTX ret = CDev::ioctl(filp, cmd, arg); +#else + ret = VDev::ioctl(filp, cmd, arg); +#endif + } return ret; } int -HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) +HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = OK; // int channel; @@ -675,8 +694,8 @@ hil_new_mode(PortMode new_mode) case PORT_MODE_UNDEFINED: case PORT1_MODE_UNSET: case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; + /* nothing more to do here */ + break; case PORT1_FULL_PWM: /* select 4-pin PWM mode */ @@ -695,20 +714,20 @@ hil_new_mode(PortMode new_mode) servo_mode = HIL::MODE_2PWM; break; - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; } // /* adjust GPIO config for serial mode(s) */ @@ -746,32 +765,32 @@ hil_start(void) return ret; } -void +int test(void) { int fd; - fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); + fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); if (fd < 0) { puts("open fail"); - exit(1); + return -ENODEV; } - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + px4_ioctl(fd, PWM_SERVO_ARM, 0); + px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - close(fd); + px4_close(fd); - exit(0); + return OK; } -void +int fake(int argc, char *argv[]) { if (argc < 5) { puts("hil fake (values -100 .. 100)"); - exit(1); + return -EINVAL; } actuator_controls_s ac; @@ -788,24 +807,39 @@ fake(int argc, char *argv[]) if (handle < 0) { puts("advertise failed"); - exit(1); + return 1; } - exit(0); + return 0; } } // namespace extern "C" __EXPORT int hil_main(int argc, char *argv[]); +static void +usage() { + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); +} + int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb = argv[1]; + const char *verb; + int ret = OK; - if (hil_start() != OK) - errx(1, "failed to start the HIL driver"); + if (hil_start() != OK) { + warnx("failed to start the HIL driver"); + return 1; + } + + if (argc < 2) { + usage(); + return -EINVAL; + } + verb = argv[1]; /* * Mode switches. @@ -842,14 +876,17 @@ hil_main(int argc, char *argv[]) return hil_new_mode(new_mode); } - if (!strcmp(verb, "test")) - test(); + if (!strcmp(verb, "test")) { + ret = test(); + } - if (!strcmp(verb, "fake")) - fake(argc - 1, argv + 1); + else if (!strcmp(verb, "fake")) { + ret = fake(argc - 1, argv + 1); + } - - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); - return -EINVAL; + else { + usage(); + ret = -EINVAL; + } + return ret; } diff --git a/src/drivers/hil/hil_posix.cpp b/src/drivers/hil/hil_posix.cpp deleted file mode 100644 index eb86a49742..0000000000 --- a/src/drivers/hil/hil_posix.cpp +++ /dev/null @@ -1,873 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 hil_linux.cpp - * - * Driver/configurator for the virtual HIL port. - * - * This virtual driver emulates PWM / servo outputs for setups where - * the connected hardware does not provide enough or no PWM outputs. - * - * Its only function is to take actuator_control uORB messages, - * mix them with any loaded mixer and output the result to the - * actuator_output uORB topic. HIL can also be performed with normal - * PWM outputs, a special flag prevents the outputs to be operated - * during HIL mode. If HIL is not performed with a standalone FMU, - * but in a real system, it is NOT recommended to use this virtual - * driver. Use instead the normal FMU or IO driver. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - -class HIL : public device::VDev -{ -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, - MODE_NONE - }; - HIL(); - virtual ~HIL(); - - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - - virtual int init(); - - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - int _update_rate; - int _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - -}; - -namespace -{ - -HIL *g_hil; - -} // namespace - -HIL::HIL() : - VDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), - _mode(MODE_NONE), - _update_rate(50), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -HIL::~HIL() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - px4_task_delete(_task); - break; - } - - } while (_task != -1); - } - - // XXX already claimed with CDEV - // /* clean up the alternate device node */ - // if (_primary_pwm_device) - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_hil = nullptr; -} - -int -HIL::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = VDev::init(); - - if (ret != PX4_OK) - return ret; - - // XXX already claimed with CDEV - ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); - if (ret == PX4_OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - // gpio_reset(); - - /* start the HIL interface task */ - _task = px4_task_spawn_cmd("fmuhil", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1200, - (px4_main_t)&HIL::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return PX4_OK; -} - -void -HIL::task_main_trampoline(int argc, char *argv[]) -{ - g_hil->task_main(); -} - -int -HIL::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - _update_rate = 10; - break; - - default: - return -EINVAL; - } - _mode = mode; - return PX4_OK; -} - -int -HIL::set_pwm_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) - return -EINVAL; - - _update_rate = rate; - return PX4_OK; -} - -void -HIL::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_LOW); - - px4_pollfd_struct_t fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) - update_rate_in_ms = 2; - orb_set_interval(_t_actuators, update_rate_in_ms); - // up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; - } - - /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&fds[0], 2, 1000); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } - } - - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - } - } - - px4_close(_t_actuators); - px4_close(_t_armed); - - /* make sure servos are off */ - // up_pwm_servo_deinit(); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; -} - -int -HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -int -HIL::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) -{ - int ret; - - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(handlep, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - ret = HIL::pwm_ioctl(handlep, cmd, arg); - break; - default: - ret = -ENOTTY; - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let VDev have it */ - if (ret == -ENOTTY) - ret = VDev::ioctl(handlep, cmd, arg); - - return ret; -} - -int -HIL::pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) -{ - int ret = PX4_OK; - // int channel; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - // up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - // up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - g_hil->set_pwm_rate(arg); - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - break; - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - // channel = cmd - PWM_SERVO_SET(0); -// up_pwm_servo_set(channel, arg); XXX - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - - } else { - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNDEFINED = 0, - PORT1_MODE_UNSET, - PORT1_FULL_PWM, - PORT1_PWM_AND_SERIAL, - PORT1_PWM_AND_GPIO, - PORT2_MODE_UNSET, - PORT2_8PWM, - PORT2_12PWM, - PORT2_16PWM, -}; - -PortMode g_port_mode; - -int -hil_new_mode(PortMode new_mode) -{ - // uint32_t gpio_bits; - - -// /* reset to all-inputs */ -// g_hil->ioctl(0, GPIO_RESET, 0); - - // gpio_bits = 0; - - HIL::Mode servo_mode = HIL::MODE_NONE; - - switch (new_mode) { - case PORT_MODE_UNDEFINED: - case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT1_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = HIL::MODE_4PWM; - break; - - case PORT1_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; -// /* set RX/TX multi-GPIOs to serial mode */ -// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT1_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; - break; - - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; - - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; - - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; - } - -// /* adjust GPIO config for serial mode(s) */ -// if (gpio_bits != 0) -// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_hil->set_mode(servo_mode); - - return PX4_OK; -} - -int -hil_start(void) -{ - int ret = PX4_OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != PX4_OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - -int -test(void) -{ - int fd; - - fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - puts("open fail"); - return -ENODEV; - } - - px4_ioctl(fd, PWM_SERVO_ARM, 0); - px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - - px4_close(fd); - - return PX4_OK; -} - -int -fake(int argc, char *argv[]) -{ - if (argc < 5) { - puts("hil fake (values -100 .. 100)"); - return -EINVAL; - } - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle < 0) { - puts("advertise failed"); - return 1; - } - - return 0; -} - -} // namespace - -extern "C" __EXPORT int hil_main(int argc, char *argv[]); - -void -usage() { - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); -} - -int -hil_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb; - int ret = PX4_OK; - - if (hil_start() != PX4_OK) { - warnx("failed to start the HIL driver"); - return 1; - } - - if (argc < 2) { - usage(); - return -EINVAL; - } - verb = argv[1]; - /* - * Mode switches. - */ - - // this was all cut-and-pasted from the FMU driver; it's junk - if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT1_FULL_PWM; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(verb, "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNDEFINED) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return PX4_OK; - - /* switch modes */ - return hil_new_mode(new_mode); - } - - if (!strcmp(verb, "test")) { - ret = test(); - } - - else if (!strcmp(verb, "fake")) { - ret = fake(argc - 1, argv + 1); - } - - else { - usage(); - ret = -EINVAL; - } - return ret; -} diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index b168108b3e..77213b4be7 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -37,10 +37,6 @@ MODULE_COMMAND = hil -ifeq ($(PX4_TARGET_OS),nuttx) SRCS = hil.cpp MAXOPTIMIZATION = -Os -else -SRCS = hil_posix.cpp -endif diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index f756250071..924624cebb 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,10 +4,6 @@ MODULE_COMMAND = rgbled -ifeq ($(PX4_TARGET_OS),nuttx) SRCS = rgbled.cpp -else -SRCS = rgbled_posix.cpp -endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 999983467b..85049abc1f 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -41,6 +41,7 @@ */ #include +#include #include @@ -54,7 +55,7 @@ #include #include -#include +#include #include #include @@ -88,7 +89,7 @@ public: virtual int init(); virtual int probe(); virtual int info(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: work_s _work; @@ -129,7 +130,11 @@ void rgbled_usage(); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : - I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000 /* maximum speed supported */), + I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled +#ifdef __PX4_NUTTX + ,100000 /* maximum speed supported */ +#endif + ), _mode(RGBLED_MODE_OFF), _r(0), _g(0), @@ -218,7 +223,7 @@ RGBLED::info() } int -RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +RGBLED::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = ENOTTY; @@ -249,7 +254,11 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* see if the parent class can make any use of it */ +#ifdef __PX4_NUTTX ret = CDev::ioctl(filp, cmd, arg); +#else + ret = VDev::ioctl(filp, cmd, arg); +#endif break; } @@ -582,35 +591,39 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - rgbledadr = strtol(optarg, NULL, 0); + rgbledadr = strtol(myoptarg, NULL, 0); break; case 'b': - i2cdevice = strtol(optarg, NULL, 0); + i2cdevice = strtol(myoptarg, NULL, 0); break; default: rgbled_usage(); - exit(0); + return 1; } } - if (optind >= argc) { + if (myoptind >= argc) { rgbled_usage(); - exit(1); + return 1; } - const char *verb = argv[optind]; + const char *verb = argv[myoptind]; int fd; int ret; if (!strcmp(verb, "start")) { - if (g_rgbled != nullptr) - errx(1, "already started"); + if (g_rgbled != nullptr) { + warnx("already started"); + return 1; + } if (i2cdevice == -1) { // try the external bus first @@ -625,7 +638,8 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { - errx(1, "init failed"); + warnx("init failed"); + return 1; } i2cdevice = PX4_I2C_BUS_LED; } @@ -634,88 +648,95 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { g_rgbled = new RGBLED(i2cdevice, rgbledadr); - if (g_rgbled == nullptr) - errx(1, "new failed"); + if (g_rgbled == nullptr) { + warnx("new failed"); + return 1; + } if (OK != g_rgbled->init()) { delete g_rgbled; g_rgbled = nullptr; - errx(1, "init failed"); + warnx("init failed"); + return 1; } } - exit(0); + return 0; } /* need the driver past this point */ if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(1); + return 1; } if (!strcmp(verb, "test")) { - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern }; - ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + ret = px4_ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); - close(fd); - exit(ret); + px4_close(fd); + return ret; } if (!strcmp(verb, "info")) { g_rgbled->info(); - exit(0); + return 0; } if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); - close(fd); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + px4_close(fd); /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { delete g_rgbled; g_rgbled = nullptr; - exit(0); + return 0; } - exit(ret); + return ret; } if (!strcmp(verb, "rgb")) { if (argc < 5) { - errx(1, "Usage: rgbled rgb "); + warnx("Usage: rgbled rgb "); + return 1; } - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } rgbled_rgbset_t v; v.red = strtol(argv[2], NULL, 0); v.green = strtol(argv[3], NULL, 0); v.blue = strtol(argv[4], NULL, 0); - ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); - close(fd); - exit(ret); + ret = px4_ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + px4_close(fd); + return ret; } rgbled_usage(); - exit(0); + return 1; } diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 851ac9020d..f739e4caec 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -46,14 +46,8 @@ SRCS = commander.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp \ - PreflightCheck.cpp - -ifdef ($(PX4_TARGET_OS),nuttx) -SRCS += + PreflightCheck.cpp \ state_machine_helper.cpp -else -SRCS += state_machine_helper_posix.cpp -endif MODULE_STACKSIZE = 5000 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7410baca3e..3b9d5fdc75 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -39,6 +39,7 @@ * @author Julian Oes */ +#include #include #include #include @@ -129,10 +130,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s prearm_ret = prearm_check(status, mavlink_fd); } +#ifdef __PX4_NUTTX /* * Perform an atomic state update */ irqstate_t flags = irqsave(); +#endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -236,8 +239,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = new_arming_state; } +#ifdef __PX4_NUTTX /* end of atomic state update */ irqrestore(flags); +#endif } if (ret == TRANSITION_DENIED) { @@ -337,6 +342,121 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } +#ifdef PX4_NUTTX +static transition_result_t disable_publication(const int mavlink_fd) +{ + transition_result_t ret; + + /* Disable publication of all attached sensors */ + /* list directory */ + DIR *d; + d = opendir("/dev"); + + if (d) { + struct dirent *direntry; + char devname[24]; + + while ((direntry = readdir(d)) != NULL) { + + /* skip serial ports */ + if (!strncmp("tty", direntry->d_name, 3)) { + continue; + } + + /* skip mtd devices */ + if (!strncmp("mtd", direntry->d_name, 3)) { + continue; + } + + /* skip ram devices */ + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { + continue; + } + + /* skip mavlink */ + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + + + } else { + /* failed opening dir */ + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; + } + return ret; +} + +#else + +static transition_result_t disable_publication(const int mavlink_fd) +{ + transition_result_t ret; + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; + + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } + + + int sensfd = px4_open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + + return ret; +} +#endif + /** * Transition from one hil state to another */ @@ -359,77 +479,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - /* Disable publication of all attached sensors */ - /* list directory */ - DIR *d; - d = opendir("/dev"); - - if (d) { - struct dirent *direntry; - char devname[24]; - - while ((direntry = readdir(d)) != NULL) { - - /* skip serial ports */ - if (!strncmp("tty", direntry->d_name, 3)) { - continue; - } - - /* skip mtd devices */ - if (!strncmp("mtd", direntry->d_name, 3)) { - continue; - } - - /* skip ram devices */ - if (!strncmp("ram", direntry->d_name, 3)) { - continue; - } - - /* skip MMC devices */ - if (!strncmp("mmc", direntry->d_name, 3)) { - continue; - } - - /* skip mavlink */ - if (!strcmp("mavlink", direntry->d_name)) { - continue; - } - - /* skip console */ - if (!strcmp("console", direntry->d_name)) { - continue; - } - - /* skip null */ - if (!strcmp("null", direntry->d_name)) { - continue; - } - - snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); - - int sensfd = ::open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - closedir(d); - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - - - } else { - /* failed opening dir */ - mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); - ret = TRANSITION_DENIED; - } + ret = disable_publication(mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; diff --git a/src/modules/mavlink/mavlink_main_nuttx.cpp b/src/modules/mavlink/mavlink_main.cpp similarity index 95% rename from src/modules/mavlink/mavlink_main_nuttx.cpp rename to src/modules/mavlink/mavlink_main.cpp index ce80953e2d..5aa3a10a5d 100644 --- a/src/modules/mavlink/mavlink_main_nuttx.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -50,7 +51,9 @@ #include #include #include +#ifndef __PX4_QURT #include +#endif #include #include /* isinf / isnan checks */ @@ -68,7 +71,7 @@ #include #include #include -#include +//#include #include #include @@ -96,8 +99,10 @@ static const int ERROR = -1; static Mavlink *_mavlink_instances = nullptr; +#ifdef __PX4_NUTTX /* TODO: if this is a class member it crashes */ static struct file_operations fops; +#endif static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; @@ -114,6 +119,9 @@ extern mavlink_system_t mavlink_system; static void usage(void); Mavlink::Mavlink() : +#ifndef __PX4_NUTTX + VDev("mavlink-log", MAVLINK_LOG_DEVICE), +#endif _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), next(nullptr), @@ -140,7 +148,9 @@ Mavlink::Mavlink() : _forwarding_on(false), _passing_on(false), _ftp_on(false), +#ifndef __PX4_QURT _uart_fd(-1), +#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -175,7 +185,9 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { +#ifdef __PX4_NUTTX fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; +#endif _instance_id = Mavlink::instance_count(); @@ -216,7 +228,8 @@ Mavlink::Mavlink() : #endif default: - errx(1, "instance ID is out of range"); + warnx("instance ID is out of range"); + px4_task_exit(1); break; } @@ -398,6 +411,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } +#ifndef __PX4_QURT int Mavlink::get_uart_fd(unsigned index) { @@ -415,6 +429,7 @@ Mavlink::get_uart_fd() { return _uart_fd; } +#endif // __PX4_QURT int Mavlink::get_instance_id() @@ -433,7 +448,11 @@ Mavlink::get_channel() ****************************************************************************/ int -Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +#ifdef __PX4_NUTTX +Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg) +#else +Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg) +#endif { switch (cmd) { case (int)MAVLINK_IOC_SEND_TEXT_INFO: @@ -549,6 +568,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } +#ifndef __PX4_QURT int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -604,7 +624,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); if (_uart_fd < 0) { return _uart_fd; @@ -619,7 +639,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -635,7 +655,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -643,7 +663,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -653,7 +673,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * * which is not an issue, but requires a separate call so we can fail silently. */ (void)tcgetattr(_uart_fd, &uart_config); +#ifdef CRTS_IFLOW uart_config.c_cflag |= CRTS_IFLOW; +#else + uart_config.c_cflag |= CRTSCTS; +#endif (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); /* setup output flow control */ @@ -696,6 +720,8 @@ Mavlink::enable_flow_control(bool enabled) return ret; } +#endif + int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -727,7 +753,13 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; + +#ifndef __PX4_QURT + +// No FIONWRITE on Linux +#if !defined(__PX4_LINUX) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { /* Disable hardware flow control: @@ -741,6 +773,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } +#endif return buf_free; } @@ -795,8 +828,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ - ssize_t ret = write(_uart_fd, buf, packet_len); + ssize_t ret = ::write(_uart_fd, buf, packet_len); if (ret != (int) packet_len) { count_txerr(); @@ -806,6 +840,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -845,8 +880,9 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ - ssize_t ret = write(_uart_fd, buf, packet_len); + ssize_t ret = ::write(_uart_fd, buf, packet_len); if (ret != (int) packet_len) { count_txerr(); @@ -856,6 +892,7 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -1195,38 +1232,42 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; +#ifdef __PX4_NUTTX /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; +#endif /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; + int myoptind=1; + const char *myoptarg = NULL; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': - _baudrate = strtoul(optarg, NULL, 10); + _baudrate = strtoul(myoptarg, NULL, 10); if (_baudrate < 9600 || _baudrate > 921600) { - warnx("invalid baud rate '%s'", optarg); + warnx("invalid baud rate '%s'", myoptarg); err_flag = true; } break; case 'r': - _datarate = strtoul(optarg, NULL, 10); + _datarate = strtoul(myoptarg, NULL, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", optarg); + warnx("invalid data rate '%s'", myoptarg); err_flag = true; } break; case 'd': - _device_name = optarg; + _device_name = myoptarg; break; // case 'e': @@ -1234,13 +1275,13 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "custom") == 0) { + if (strcmp(myoptarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; - } else if (strcmp(optarg, "camera") == 0) { + } else if (strcmp(myoptarg, "camera") == 0) { // left in here for compatibility _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(optarg, "onboard") == 0) { + } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; } @@ -1296,6 +1337,7 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); +#ifndef __PX4_QURT struct termios uart_config_original; /* default values for arguments */ @@ -1305,6 +1347,7 @@ Mavlink::task_main(int argc, char *argv[]) warn("could not open %s", _device_name); return ERROR; } +#endif /* initialize send mutex */ pthread_mutex_init(&_send_mutex, NULL); @@ -1319,7 +1362,8 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - errx(1, "msg buf:"); + warnx("msg buf:"); + return 1; } /* initialize message buffer mutex */ @@ -1327,10 +1371,14 @@ Mavlink::task_main(int argc, char *argv[]) } /* create the device node that's used for sending text log messages, etc. */ +#ifdef __PX4_NUTTX register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); +#else + register_driver(MAVLINK_LOG_DEVICE, NULL); +#endif /* initialize logging device */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* Initialize system properties */ mavlink_update_system(); @@ -1556,14 +1604,16 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); +#ifndef __PX4_QURT /* reset the UART flags to original state */ tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ - close(_uart_fd); + ::close(_uart_fd); +#endif /* close mavlink logging device */ - close(_mavlink_fd); + px4_close(_mavlink_fd); if (_passing_on || _ftp_on) { message_buffer_destroy(); @@ -1618,11 +1668,11 @@ Mavlink::start(int argc, char *argv[]) // between the starting task and the spawned // task - start_helper() only returns // when the started task exits. - task_spawn_cmd(buf, + px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2400, - (main_t)&Mavlink::start_helper, + (px4_main_t)&Mavlink::start_helper, (char * const *)argv); // Ensure that this shell command @@ -1653,7 +1703,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time)); } printf("\tmavlink chan: #%u\n", _channel); @@ -1743,11 +1793,13 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. - errx(0, "mavlink for device %s is not running", device_name); + warnx("mavlink for device %s is not running", device_name); + return 0; } } else { - errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] -s stream -r rate"); + return 1; } return OK; @@ -1762,7 +1814,7 @@ int mavlink_main(int argc, char *argv[]) { if (argc < 2) { usage(); - exit(1); + return 1; } if (!strcmp(argv[1], "start")) { @@ -1771,7 +1823,7 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); usage(); - exit(1); + return 1; } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); @@ -1784,7 +1836,7 @@ int mavlink_main(int argc, char *argv[]) } else { usage(); - exit(1); + return 1; } return 0; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 86b259a91b..87e8be4362 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -421,7 +421,7 @@ private: #ifdef __PX4_NUTTX static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); #else - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); #endif /** diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index 5147368f41..9ac7707cfd 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -437,7 +437,7 @@ Mavlink::get_channel() ****************************************************************************/ int -Mavlink::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case (int)MAVLINK_IOC_SEND_TEXT_INFO: diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 41873e9d43..0b035f0258 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -35,14 +35,8 @@ # MAVLink protocol to uORB interface process # -MODULE_COMMAND = mavlink -ifeq ($(PX4_TARGET_OS),nuttx) -SRCS += mavlink_main_nuttx.cpp -else -SRCS += mavlink_main_posix.cpp -endif - SRCS += mavlink.c \ + mavlink_main.cpp \ mavlink_mission.cpp \ mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index e55e137d61..528212dc12 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -121,16 +121,16 @@ public: ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); - virtual int open(device::px4_dev_handle_t *handlep); - virtual int close(device::px4_dev_handle_t *handlep); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int open(device::file_t *filp); + virtual int close(device::file_t *filp); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); protected: - virtual pollevent_t poll_state(device::px4_dev_handle_t *handlep); + virtual pollevent_t poll_state(device::file_t *filp); virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: @@ -150,7 +150,7 @@ private: pid_t _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ - SubscriberData *handlep_to_sd(device::px4_dev_handle_t *handlep); + SubscriberData *filp_to_sd(device::file_t *filp); /** * Perform a deferred update for a rate-limited subscriber. @@ -173,16 +173,13 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::SubscriberData *ORBDevNode::handlep_to_sd(device::px4_dev_handle_t *handlep) +ORBDevNode::SubscriberData *ORBDevNode::filp_to_sd(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::handlep_to_sd %p\n", handlep); ORBDevNode::SubscriberData *sd; - if (handlep) { - sd = (ORBDevNode::SubscriberData *)(handlep->priv); - PX4_DEBUG(" sd = %p \n", sd); + if (filp) { + sd = (ORBDevNode::SubscriberData *)(filp->priv); } else { - PX4_DEBUG("*** ERROR ORBDevNode::handlep_to_sd(0)\n"); sd = 0; } return sd; @@ -209,12 +206,12 @@ ORBDevNode::~ORBDevNode() } int -ORBDevNode::open(device::px4_dev_handle_t *handlep) +ORBDevNode::open(device::file_t *filp) { int ret; /* is this a publisher? */ - if (handlep->flags == PX4_F_WRONLY) { + if (filp->flags == PX4_F_WRONLY) { /* become the publisher if we can */ lock(); @@ -231,7 +228,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* now complete the open */ if (ret == PX4_OK) { - ret = VDev::open(handlep); + ret = VDev::open(filp); /* open failed - not the publisher anymore */ if (ret != PX4_OK) @@ -242,7 +239,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } /* is this a new subscriber? */ - if (handlep->flags == PX4_F_RDONLY) { + if (filp->flags == PX4_F_RDONLY) { /* allocate subscriber data */ SubscriberData *sd = new SubscriberData; @@ -258,16 +255,16 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* set priority */ sd->priority = _priority; - handlep->priv = (void *)sd; + filp->priv = (void *)sd; - ret = VDev::open(handlep); + ret = VDev::open(filp); if (ret != PX4_OK) { - PX4_DEBUG("ERROR: VDev::open failed\n"); + warnx("ERROR: VDev::open failed\n"); delete sd; } - PX4_DEBUG("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", handlep->fd, handlep->flags, handlep->priv, handlep->cdev); + //warnx("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); return ret; } @@ -276,31 +273,30 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } int -ORBDevNode::close(device::px4_dev_handle_t *handlep) +ORBDevNode::close(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::close fd = %d\n", handlep->fd); + //warnx("ORBDevNode::close fd = %d", filp->fd); /* is this the publisher closing? */ if (getpid() == _publisher) { _publisher = 0; } else { - SubscriberData *sd = handlep_to_sd(handlep); + SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - PX4_DEBUG(" delete handlep->priv %p", handlep->priv); hrt_cancel(&sd->update_call); delete sd; } } - return VDev::close(handlep); + return VDev::close(filp); } ssize_t -ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) { - PX4_DEBUG("ORBDevNode::read fd = %d\n", handlep->fd); - SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep); + //warnx("ORBDevNode::read fd = %d\n", filp->fd); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); /* if the object has not been written yet, return zero */ if (_data == nullptr) @@ -338,9 +334,9 @@ ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } ssize_t -ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen) { - PX4_DEBUG("ORBDevNode::write handlep = %p (null is normal)\n", handlep); + //warnx("ORBDevNode::write filp = %p (null is normal)", filp); /* * Writes are legal from interrupt context as long as the * object has already been initialised from thread context. @@ -348,7 +344,7 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t * Writes outside interrupt context will allocate the object * if it has not yet been allocated. * - * Note that handlep will usually be NULL. + * Note that filp will usually be NULL. */ if (nullptr == _data) { lock(); @@ -385,10 +381,10 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t } int -ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - PX4_DEBUG("ORBDevNode::ioctl fd = %d cmd = %d\n", handlep->fd, cmd); - SubscriberData *sd = handlep_to_sd(handlep); + //warnx("ORBDevNode::ioctl fd = %d cmd = %d", filp->fd, cmd); + SubscriberData *sd = filp_to_sd(filp); switch (cmd) { case ORBIOCLASTUPDATE: @@ -413,17 +409,17 @@ ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } ssize_t ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) { - PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta); + //warnx("ORBDevNode::publish meta = %p", meta); if (handle < 0) { - PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta); + warnx("ORBDevNode::publish called with invalid handle"); errno = EINVAL; return ERROR; } @@ -452,10 +448,10 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d } pollevent_t -ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) +ORBDevNode::poll_state(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::poll_state fd = %d\n", handlep->fd); - SubscriberData *sd = handlep_to_sd(handlep); + //warnx("ORBDevNode::poll_state fd = %d", filp->fd); + SubscriberData *sd = filp_to_sd(filp); /* * If the topic appears updated to the subscriber, say so. @@ -469,8 +465,8 @@ ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) void ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - PX4_DEBUG("ORBDevNode::poll_notify_one fds = %p fds->priv = %p\n", fds, fds->priv); - SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv); + //warnx("ORBDevNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); + SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); /* * If the topic looks updated to the subscriber, go ahead and notify them. @@ -482,7 +478,7 @@ ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) bool ORBDevNode::appears_updated(SubscriberData *sd) { - PX4_DEBUG("ORBDevNode::appears_updated sd = %p\n", sd); + //warnx("ORBDevNode::appears_updated sd = %p", sd); /* assume it doesn't look updated */ bool ret = false; @@ -583,7 +579,7 @@ public: ORBDevMaster(Flavor f); ~ORBDevMaster(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: Flavor _flavor; }; @@ -603,7 +599,7 @@ ORBDevMaster::~ORBDevMaster() } int -ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret; @@ -696,7 +692,7 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1242,7 +1238,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - PX4_DEBUG("orb_advertise meta = %p\n", meta); + //warnx("orb_advertise meta = %p", meta); return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); } @@ -1252,29 +1248,27 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst int result, fd; orb_advert_t advertiser; - PX4_DEBUG("orb_advertise_multi meta = %p\n", meta); + //warnx("orb_advertise_multi meta = %p\n", meta); /* open the node as an advertiser */ fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) { - PX4_DEBUG(" node_open as advertiser failed.\n"); + warnx("node_open as advertiser failed."); return ERROR; } - PX4_DEBUG(" node_open as advertiser passed. fd = %d, instance = %p %d\n", fd, instance, instance != 0 ? *instance : 0); - /* get the advertiser handle and close the node */ result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == ERROR) { - PX4_DEBUG(" px4_ioctl ORBIOCGADVERTISER failed. fd = %d\n", fd); + warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); return ERROR; } /* the advertiser must perform an initial publish to initialise the object */ result = orb_publish(meta, advertiser, data); if (result == ERROR) { - PX4_DEBUG(" orb_publish failed\n"); + warnx("orb_publish failed"); return ERROR; } diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 704c7fc088..b108f4969b 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -100,8 +100,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -116,8 +116,8 @@ public: protected: friend class ACCELSIM_mag; - virtual ssize_t mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -325,8 +325,8 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -516,7 +516,7 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } ssize_t -ACCELSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); @@ -553,7 +553,7 @@ ACCELSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } ssize_t -ACCELSIM::mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(buffer); @@ -592,7 +592,7 @@ ACCELSIM::mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t bufle } int -ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -614,10 +614,10 @@ ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(handlep, SENSORIOCSPOLLRATE, 1600); + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(handlep, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -712,12 +712,12 @@ ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } int -ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -741,7 +741,7 @@ ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(handlep, SENSORIOCSPOLLRATE, 100); + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { @@ -831,7 +831,7 @@ ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar return OK; default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1209,20 +1209,20 @@ ACCELSIM_mag::parent_poll_notify() } ssize_t -ACCELSIM_mag::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) { - return _parent->mag_read(handlep, buffer, buflen); + return _parent->mag_read(filp, buffer, buflen); } int -ACCELSIM_mag::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(handlep, cmd, arg); + return (int)VDev::ioctl(filp, cmd, arg); break; default: - return _parent->mag_ioctl(handlep, cmd, arg); + return _parent->mag_ioctl(filp, cmd, arg); } } diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 1e859c386e..893c30948c 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -74,12 +74,12 @@ public: virtual int init(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t len); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); protected: - virtual int open_first(device::px4_dev_handle_t *handlep); - virtual int close_last(device::px4_dev_handle_t *handlep); + virtual int open_first(device::file_t *filp); + virtual int close_last(device::file_t *filp); private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ @@ -157,13 +157,13 @@ ADCSIM::init() } int -ADCSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { return -ENOTTY; } ssize_t -ADCSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t len) +ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; @@ -177,7 +177,7 @@ ADCSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t len) } int -ADCSIM::open_first(device::px4_dev_handle_t *handlep) +ADCSIM::open_first(device::file_t *filp) { /* get fresh data */ _tick(); @@ -189,7 +189,7 @@ ADCSIM::open_first(device::px4_dev_handle_t *handlep) } int -ADCSIM::close_last(device::px4_dev_handle_t *handlep) +ADCSIM::close_last(device::file_t *filp) { hrt_cancel(&_call); return 0; diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index bef9c9c5aa..eb6c6c29f7 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -100,8 +100,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -327,7 +327,7 @@ out: } ssize_t -BAROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); struct baro_report *brp = reinterpret_cast(buffer); @@ -397,7 +397,7 @@ BAROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } int -BAROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -502,7 +502,7 @@ BAROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } void diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 55a34b191a..44e85150c0 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -197,8 +197,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -211,8 +211,8 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); + virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); private: GYROSIM_gyro *_gyro; @@ -438,8 +438,8 @@ public: GYROSIM_gyro(GYROSIM *parent, const char *path); ~GYROSIM_gyro(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -733,7 +733,7 @@ GYROSIM::_set_dlpf_filter(uint16_t frequency_hz) } ssize_t -GYROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -856,7 +856,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -891,7 +891,7 @@ GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t bufle } int -GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -916,10 +916,10 @@ GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(handlep, SENSORIOCSPOLLRATE, 1000); + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(handlep, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -1030,12 +1030,12 @@ GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } int -GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -1043,7 +1043,7 @@ GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(handlep, cmd, arg); + return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -1101,7 +1101,7 @@ GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1578,21 +1578,21 @@ GYROSIM_gyro::parent_poll_notify() } ssize_t -GYROSIM_gyro::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) { - return _parent->gyro_read(handlep, buffer, buflen); + return _parent->gyro_read(filp, buffer, buflen); } int -GYROSIM_gyro::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(handlep, cmd, arg); + return (int)VDev::ioctl(filp, cmd, arg); break; default: - return _parent->gyro_ioctl(handlep, cmd, arg); + return _parent->gyro_ioctl(filp, cmd, arg); } } diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index b4cb41a4fd..b891ab9982 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -46,24 +46,30 @@ #include #include +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +#ifdef __PX4_NUTTX + +typedef struct pollfd px4_pollfd_struct_t; + +#if defined(__cplusplus) +#define _GLOBAL :: +#else +#define _GLOBAL +#endif +#define px4_open _GLOBAL open +#define px4_close _GLOBAL close +#define px4_ioctl _GLOBAL ioctl +#define px4_write _GLOBAL write +#define px4_read _GLOBAL read +#define px4_poll _GLOBAL poll + +#elif defined(__PX4_POSIX) #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 -#define PX4_DEBUG(...) -//#define PX4_DEBUG(...) printf(__VA_ARGS__) - -#ifdef __PX4_NUTTX - -#define px4_pollfd_struct_t struct pollfd -#define px4_open open -#define px4_close close -#define px4_ioctl ioctl -#define px4_write write -#define px4_read read -#define px4_poll poll - -#else typedef short pollevent_t; typedef struct { @@ -87,7 +93,10 @@ __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __END_DECLS +#else +#error "No TARGET OS Provided" #endif + __BEGIN_DECLS extern int px4_errno;