From 126b0567feb7384ce30997f01a5e9eb873e018aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:24:16 +0100 Subject: [PATCH 1/3] add safety check for orbit in navigator. Prevents issues with old qgc versions --- src/modules/navigator/navigator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 399985442b..936e0589cf 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -952,13 +952,14 @@ Navigator::check_mission_item_reached() uint64_t now = hrt_absolute_time(); float orbit; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { orbit = _mission_item_triplet.current.radius; } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED && + _mission_item_triplet.current.loiter_radius > 0.01f) { orbit = _mission_item_triplet.current.loiter_radius; } else { From b66730b5a9011e349d25655f777dccf5803d90c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:38:53 +0100 Subject: [PATCH 2/3] making sure the mavlink fd is open in fw pos ctrl --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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 7da28cbfa0..d12a1750ab 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 @@ -1173,6 +1173,11 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; From 254777d38ae0ab30d7f8f75e49a3619aae592460 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:49:59 +0100 Subject: [PATCH 3/3] more fixes for the mavlink_fd in fw pos ctrl, this now enables mavlink output for landing --- .../fw_pos_control_l1_main.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) 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 d12a1750ab..d60983bce2 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 @@ -95,8 +95,6 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); -static int mavlink_fd; - class FixedwingPositionControl { public: @@ -118,6 +116,7 @@ public: int start(); private: + int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -377,11 +376,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - flare_curve_alt_last(0.0f) + flare_curve_alt_last(0.0f), + _mavlink_fd(-1) { - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* safely initialize structs */ vehicle_attitude_s _att = {0}; vehicle_attitude_setpoint_s _att_sp = {0}; @@ -884,7 +881,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, limit throttle"); } } @@ -905,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio flare_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, flare"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -924,7 +921,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (!land_onslope) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, on slope"); land_onslope = true; } @@ -1174,9 +1171,9 @@ FixedwingPositionControl::task_main() if (fds[1].revents & POLLIN) { /* XXX Hack to get mavlink output going */ - if (mavlink_fd < 0) { + if (_mavlink_fd < 0) { /* try to open the mavlink log device every once in a while */ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static uint64_t last_run = 0;