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); } } }