void MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); if (_count > 0) { _state = MAVLINK_WPM_STATE_SENDLIST; _transfer_seq = 0; _transfer_count = _count; _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy"); } } }
void MavlinkMissionManager::send(const hrt_abstime now) { /* update interval for slow rate limiter */ _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); if (updated) { mission_result_s mission_result; orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); _current_seq = mission_result.seq_current; if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } if (mission_result.reached) { send_mission_item_reached((uint16_t)mission_result.seq_reached); } send_mission_current(_current_seq); } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); } } /* check for timed-out operations */ if (_state != MAVLINK_WPM_STATE_IDLE && 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); } _state = MAVLINK_WPM_STATE_IDLE; } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { /* try to request item again after timeout */ send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { if (_transfer_seq == 0) { /* try to send items count again after timeout */ send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); } else { /* try to send item again after timeout */ send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); } } }
void MavlinkMissionManager::send(const hrt_abstime now) { bool updated = false; orb_check(_mission_result_sub, &updated); if (updated) { mission_result_s mission_result; orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); _current_seq = mission_result.seq_current; if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } if (mission_result.reached) { _last_reached = mission_result.seq_reached; send_mission_item_reached((uint16_t)mission_result.seq_reached); } else { _last_reached = -1; } send_mission_current(_current_seq); if (mission_result.item_do_jump_changed) { /* send a mission item again if the remaining DO_JUMPs has changed */ send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, (uint16_t)mission_result.item_changed_index); } } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); if (_last_reached >= 0) { send_mission_item_reached((uint16_t)_last_reached); } } } /* check for timed-out operations */ if (_state != MAVLINK_WPM_STATE_IDLE && 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); } _state = MAVLINK_WPM_STATE_IDLE; // since we are giving up, reset this state also, so another request can be started. _transfer_in_progress = false; } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { /* try to request item again after timeout, * toggle int32 or float protocol variant to try both */ _int_mode = !_int_mode; send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { if (_transfer_seq == 0) { /* try to send items count again after timeout */ send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); } else { /* try to send item again after timeout */ send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); } } }