diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 02:04:05 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 02:04:05 +0200 |
commit | 2ea32b315fb06f13813950a8ef385dd521694278 (patch) | |
tree | 2ea9711c8a3f18244685a6a356bfc43b3836730b /src/modules | |
parent | eb9daf097e10f434e729581b3ffce6c1b322b927 (diff) | |
parent | 0b85c41cd19486e829d48cf9e604fbc25b9e52a6 (diff) | |
download | px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.tar.gz px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.tar.bz2 px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.zip |
Merged master
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 31 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 12 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 42 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 4 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/uORB/objects_common.cpp | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission.h | 2 |
8 files changed, 85 insertions, 16 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585e..8ea611802 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -959,7 +959,7 @@ FixedwingEstimator::task_main() } // Publish results - if (_initialized) { + if (_initialized && (check == OK)) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c90b4e2de..a9f5f4de7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + // XXX do something here, but change to using FIONWRITE and OS buf size for detection + } } + + } static void usage(void); @@ -204,6 +209,8 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -825,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } @@ -1768,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1820,6 +1827,10 @@ Mavlink::task_main(int argc, char *argv[]) _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -2265,7 +2276,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9941a5f99..66d82b471 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -202,6 +202,14 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -216,6 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -270,6 +280,8 @@ private: pthread_mutex_t _message_buffer_mutex; + + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e608bf787..58af1fcb6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -641,6 +641,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1344,5 +1385,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), new MavlinkStreamDistanceSensor(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c66350f5b..b4fe65fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a94b6671..bcb3c8d0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9594c4c6a..ef4bc1def 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -105,7 +105,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif |