diff --git a/launch/ardrone.launch b/launch/ardrone.launch index f43bbf4709..db980731fe 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/iris.launch b/launch/iris.launch index 5231e3215b..798b761d76 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multi_uav.launch b/launch/multi_uav.launch new file mode 100644 index 0000000000..6fb1589e80 --- /dev/null +++ b/launch/multi_uav.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 6882f24137..bf68da95dd 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,5 +1,6 @@ + @@ -10,7 +11,9 @@ - + + + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index 66c30d186f..617357452f 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,10 @@ + + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index c686eba390..b7db46dcb5 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,10 @@ + + diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index d77082a89a..b745d0e263 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -97,7 +97,7 @@ MODULES += platforms/posix/drivers/gpssim # Unit tests # #MODULES += platforms/posix/tests/hello -#MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test #MODULES += platforms/posix/tests/wqueue diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5815629f0..c3816f5374 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1227,9 +1227,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - land_noreturn_horizontal = true; } else { @@ -1241,6 +1238,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (land_noreturn_horizontal) { + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + } /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b23ee6ea60..c37322d37b 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -133,7 +133,7 @@ private: bool _verbose; - static constexpr int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors + static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index dc3111a40a..46f4b1d82d 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -51,54 +51,128 @@ px4::AppState VCDevExample::appState; using namespace device; -#define TESTDEV "/dev/vdevex" +#define TESTDEV "/dev/vdevtest" + +static bool g_exit = false; static int writer_main(int argc, char *argv[]) { - char buf[1] = { '1' }; + char buf[1]; - int fd = px4_open(TESTDEV, PX4_F_RDONLY); + int fd = px4_open(TESTDEV, PX4_F_WRONLY); if (fd < 0) { - PX4_INFO("--- Open failed %d %d", fd, px4_errno); + PX4_INFO("Writer: Open failed %d %d", fd, px4_errno); return -px4_errno; } int ret; int i=0; - while (i<3) { - // Wait for 3 seconds - PX4_INFO("--- Sleeping for 4 sec\n"); - ret = sleep(4); + while (!g_exit) { + // Wait for 2 seconds + PX4_INFO("Writer: Sleeping for 2 sec"); + ret = sleep(2); if (ret < 0) { - PX4_INFO("--- sleep failed %d %d\n", ret, errno); + PX4_INFO("Writer: sleep failed %d %d", ret, errno); return ret; } - PX4_INFO("--- writing to fd\n"); + buf[0] = 'A'+(char)(i % 26); + PX4_INFO("Writer: writing char '%c'", buf[0]); ret = px4_write(fd, buf, 1); ++i; } px4_close(fd); + PX4_INFO("Writer: stopped"); return ret; } +class PrivData { +public: + PrivData() : _read_offset(0) {} + ~PrivData() {} + + size_t _read_offset; +}; + class VCDevNode : public VDev { public: - VCDevNode() : VDev("vcdevtest", TESTDEV) {}; + VCDevNode() : + VDev("vcdevtest", TESTDEV), + _is_open_for_write(false), + _write_offset(0) {}; ~VCDevNode() {} + virtual int open(device::file_t *handlep); + virtual int close(device::file_t *handlep); virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen); +private: + bool _is_open_for_write; + size_t _write_offset; + char _buf[1000]; }; +int VCDevNode::open(device::file_t *handlep) +{ + // Only allow one writer + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + errno = EBUSY; + return -1; + } + int ret = VDev::open(handlep); + if (ret != 0) { + return ret; + } + handlep->priv = new PrivData; + + if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + _is_open_for_write = true; + } + + return 0; +} + +int VCDevNode::close(device::file_t *handlep) +{ + delete (PrivData *)handlep->priv; + handlep->priv = nullptr; + VDev::close(handlep); + + // Enable a new writer of the device is re-opened for write + if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { + _is_open_for_write = false; + } + return 0; +} + ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { + for (size_t i=0; ipriv; + ssize_t chars_read = 0; + PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); + for (size_t i=0; i_read_offset < _write_offset); i++) { + buffer[i] = _buf[p->_read_offset]; + p->_read_offset++; + chars_read++; + } + + return chars_read; +} + VCDevExample::~VCDevExample() { if (_node) { delete _node; @@ -119,11 +193,57 @@ static int test_pub_block(int fd, unsigned long blocked) PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; } +int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll) +{ + int pollret, readret; + int loop_count = 0; + char readbuf[10]; + px4_pollfd_struct_t fds[1]; + + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + bool mustblock = (timeout < 0); + + // Test indefinte blocking poll + while ((!appState.exitRequested()) && (loop_count < iterations)) { + pollret = px4_poll(fds, 1, timeout); + if (pollret < 0) { + PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno); + goto fail; + } + PX4_INFO("Reader: px4_poll returned %d", pollret); + if (pollret) { + readret = px4_read(fd, readbuf, 10); + if (readret != 1) { + if (mustblock) { + PX4_ERR("Reader: read failed %d FAIL", readret); + goto fail; + } + else { + PX4_INFO("Reader: read failed %d FAIL", readret); + } + } + else { + readbuf[readret] = '\0'; + PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); + } + } + if (delayms_after_poll) { + usleep(delayms_after_poll*1000); + } + loop_count++; + } + return 0; +fail: + return 1; +} int VCDevExample::main() { appState.setRunning(true); @@ -131,12 +251,12 @@ int VCDevExample::main() _node = new VCDevNode(); if (_node == 0) { - PX4_INFO("Failed to allocate VCDevNode\n"); + PX4_INFO("Failed to allocate VCDevNode"); return -ENOMEM; } if (_node->init() != PX4_OK) { - PX4_INFO("Failed to init VCDevNode\n"); + PX4_INFO("Failed to init VCDevNode"); return 1; } @@ -153,7 +273,7 @@ int VCDevExample::main() PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } - PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); if (ret < 0) @@ -162,10 +282,8 @@ int VCDevExample::main() if (ret < 0) return ret; - int i=0; - px4_pollfd_struct_t fds[1]; - // Start a task that will write something in 3 seconds + // Start a task that will write something in 4 seconds (void)px4_task_spawn_cmd("writer", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 6, @@ -173,42 +291,28 @@ int VCDevExample::main() writer_main, (char* const*)NULL); - while (!appState.exitRequested() && i<13) { - PX4_INFO("=====================\n"); - PX4_INFO("==== sleeping 2 sec ====\n"); - sleep(2); + ret = 0; - fds[0].fd = fd; - fds[0].events = POLLIN; - fds[0].revents = 0; - - PX4_INFO("==== Calling Poll\n"); - ret = px4_poll(fds, 1, 1000); - PX4_INFO("==== Done poll\n"); - if (ret < 0) { - PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); - px4_close(fd); - } - else if (i > 0) { - if (ret == 0) { - PX4_INFO("==== Nothing to read - PASS\n"); - } - else { - PX4_INFO("==== poll returned %d\n", ret); - } - } - else if (i == 0) { - if (ret == 1) { - PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); - } - else { - PX4_INFO("==== %d to read - FAIL\n", ret); - } - - } - ++i; + PX4_INFO("TEST: BLOCKING POLL ---------------"); + if (do_poll(fd, -1, 3, 0)) { + ret = 1; + goto fail2; + } + PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); + if(do_poll(fd, 0, 3, 0)) { + ret = 1; + goto fail2; + } + PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); + if(do_poll(fd, 1000, 3, 0)) { + ret = 1; + goto fail2; } - px4_close(fd); - return 0; +fail2: + g_exit = true; + px4_close(fd); + PX4_INFO("TEST: waiting for writer to stop"); + sleep(3); + return ret; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h index 4898210dff..10befc795c 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h @@ -54,5 +54,7 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: + int do_poll(int fd, int timeout, int iterations, int delayms_after_poll); + VCDevNode *_node; }; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index c92dd0843d..567acc200e 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -46,7 +46,7 @@ using namespace px4; -Mavlink::Mavlink() : +Mavlink::Mavlink(std::string mavlink_fcu_url) : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), @@ -55,7 +55,7 @@ Mavlink::Mavlink() : _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)), _force_sp_pub(_n.advertise("vehicle_force_setpoint", 1)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); _att_sp = {}; _offboard_control_mode = {}; @@ -64,7 +64,10 @@ Mavlink::Mavlink() : int main(int argc, char **argv) { ros::init(argc, argv, "mavlink"); - Mavlink m; + ros::NodeHandle privateNh("~"); + std::string mavlink_fcu_url; + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560")); + Mavlink m(mavlink_fcu_url); ros::spin(); return 0; } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 8b7d08d242..af9e019993 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -55,7 +55,7 @@ namespace px4 class Mavlink { public: - Mavlink(); + Mavlink(std::string mavlink_fcu_url); ~Mavlink() {}