From 6667b6434b21704492374d1850a1cc4dfc6c9ecd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 15 May 2017 17:03:38 +0200 Subject: [PATCH] mavlink_mission: replace warnx, add mission_type to verbose output --- src/modules/mavlink/mavlink_mission.cpp | 97 +++++++++++++------------ 1 file changed, 51 insertions(+), 46 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 1345e6cc16..2b316a8583 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -193,7 +193,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int return PX4_OK; } else { - warnx("WPM: ERROR: can't save mission state"); + PX4_ERR("WPM: can't save mission state"); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); @@ -218,7 +218,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) orb_publish_auto(ORB_ID(geofence_update), &_geofence_update_pub, &geofence_update, nullptr, ORB_PRIO_DEFAULT); } else { - warnx("WPM: ERROR: can't save mission state"); + PX4_ERR("WPM: can't save mission state"); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); @@ -244,7 +244,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) _count[(uint8_t)MAV_MISSION_TYPE_RALLY] = count; } else { - warnx("WPM: ERROR: can't save mission state"); + PX4_ERR("WPM: can't save mission state"); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); @@ -268,7 +268,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); - if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } + if (_verbose) { PX4_INFO("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -286,7 +286,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) /* don't broadcast if no WPs */ } else { - if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + if (_verbose) { PX4_ERR("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } _mavlink->send_statustext_critical("ERROR: wp index out of bounds"); } @@ -307,7 +307,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); - if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } + if (_verbose) { PX4_INFO("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type); } } @@ -398,7 +398,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD"); } - if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + if (_verbose) { PX4_ERR("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } } } @@ -460,7 +460,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 } else { _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + if (_verbose) { PX4_ERR("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } } } @@ -474,7 +474,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached); - if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } + if (_verbose) { PX4_INFO("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } @@ -490,7 +490,7 @@ MavlinkMissionManager::send(const hrt_abstime now) _current_seq = mission_result.seq_current; - if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + if (_verbose) { PX4_INFO("WPM: got mission result, new current_seq: %d", _current_seq); } if (mission_result.reached) { _time_last_reached = now; @@ -543,7 +543,7 @@ MavlinkMissionManager::send(const hrt_abstime now) && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { _mavlink->send_statustext_critical("Operation timeout"); - if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + if (_verbose) { PX4_INFO("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } _state = MAVLINK_WPM_STATE_IDLE; @@ -616,12 +616,12 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (_transfer_seq == current_item_count()) { - if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + if (_verbose) { PX4_INFO("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } } else { _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE"); - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + if (_verbose) { PX4_ERR("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } } _state = MAVLINK_WPM_STATE_IDLE; @@ -641,7 +641,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); if (_verbose) { - warnx("WPM: MISSION_ACK ERR: ID mismatch"); + PX4_INFO("WPM: MISSION_ACK ERR: ID mismatch"); } } } @@ -662,10 +662,10 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: if (update_active_mission(_dataman_id, _count[(uint8_t)_mission_type], wpc.seq) == PX4_OK) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + if (_verbose) { PX4_INFO("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID"); } @@ -686,13 +686,13 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list"); } } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + if (_verbose) { PX4_ERR("WPM: MISSION_SET_CURRENT ERROR: busy"); } _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy"); } @@ -734,16 +734,21 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) _transfer_partner_compid = msg->compid; if (_transfer_count > 0) { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + if (_verbose) { + PX4_INFO("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, + (int)_mission_type); + } } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + if (_verbose) { + PX4_INFO("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", (int)_mission_type); + } } send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type); } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + if (_verbose) { PX4_ERR("WPM: MISSION_REQUEST_LIST ERROR: busy"); } _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); } @@ -794,22 +799,22 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) /* _transfer_seq contains sequence of expected request */ if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u", wpr.seq, msg->sysid); } + if (_verbose) { PX4_INFO("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u", wpr.seq, msg->sysid); } _transfer_seq++; } else if (wpr.seq == _transfer_seq - 1) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u (again)", wpr.seq, msg->sysid); } + if (_verbose) { PX4_INFO("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u (again)", wpr.seq, msg->sysid); } } else { if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } } else if (_transfer_seq <= 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } } _state = MAVLINK_WPM_STATE_IDLE; @@ -824,7 +829,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); } _state = MAVLINK_WPM_STATE_IDLE; @@ -833,13 +838,13 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) } } else if (_state == MAVLINK_WPM_STATE_IDLE) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer"); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer"); } // Silently ignore this as some OSDs have buggy mission protocol implementations //_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer"); } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state); } _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); } @@ -847,7 +852,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) } else { _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch"); - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: rejected, partner ID mismatch"); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: rejected, partner ID mismatch"); } } } } @@ -872,7 +877,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _mission_type = (MAV_MISSION_TYPE)wpc.mission_type; if (wpc.count > current_max_item_count()) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count()); } + if (_verbose) { PX4_ERR("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count()); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); _transfer_in_progress = false; @@ -880,7 +885,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } if (wpc.count == 0) { - if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + if (_verbose) { PX4_INFO("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: @@ -906,7 +911,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) return; } - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + if (_verbose) { PX4_INFO("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } _state = MAVLINK_WPM_STATE_GETLIST; _transfer_seq = 0; @@ -921,17 +926,17 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) if (_transfer_seq == 0) { /* looks like our MISSION_REQUEST was lost, try again */ - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + if (_verbose) { PX4_INFO("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy"); return; } } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + if (_verbose) { PX4_ERR("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); return; @@ -985,20 +990,20 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wp.seq != _transfer_seq) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } /* don't send request here, it will be performed in eventloop after timeout */ return; } } else if (_state == MAVLINK_WPM_STATE_IDLE) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM ERROR: no transfer"); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; @@ -1009,7 +1014,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) int ret = parse_mavlink_mission_item(&wp, &mission_item); if (ret != PX4_OK) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); @@ -1076,7 +1081,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } if (write_failed || check_failed) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + if (_verbose) { PX4_ERR("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1094,13 +1099,13 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) _transfer_current_seq = wp.seq; } - if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + if (_verbose) { PX4_INFO("WPM: MISSION_ITEM seq %u received", wp.seq); } _transfer_seq = wp.seq + 1; if (_transfer_seq == _transfer_count) { /* got all new mission items successfully */ - if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + if (_verbose) { PX4_INFO("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } _state = MAVLINK_WPM_STATE_IDLE; @@ -1181,7 +1186,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } if (ret == PX4_OK) { - if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + if (_verbose) { PX4_INFO("WPM: CLEAR_ALL OK"); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); @@ -1192,7 +1197,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } else { _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy"); - if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + if (_verbose) { PX4_ERR("WPM: CLEAR_ALL IGNORED: busy"); } } } } @@ -1537,7 +1542,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * void MavlinkMissionManager::check_active_mission() { if (!(_my_dataman_id == _dataman_id)) { - if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } + if (_verbose) { PX4_INFO("WPM: New mission detected (possibly over different Mavlink instance) Updating"); } _my_dataman_id = _dataman_id; send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION],