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::handle_mission_item(const mavlink_message_t *msg) { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _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); } /* 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"); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } struct mission_item_s mission_item; int ret = parse_mavlink_mission_item(&wp, &mission_item); if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (_verbose) { warnx("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); _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } /* waypoint marked as current */ if (wp.current) { _transfer_current_seq = wp.seq; } if (_verbose) { warnx("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); } _mavlink->send_statustext_info("WPM: Transfer complete."); _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); } _transfer_in_progress = false; } else { /* request next item */ send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } } }
void MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); if(_transfer_in_progress) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); return; } _transfer_in_progress = true; if (wpc.count > _max_count) { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); _transfer_in_progress = false; return; } if (wpc.count == 0) { if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); // TODO send ACK? _transfer_in_progress = false; return; } if (_verbose) { warnx("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; _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission _transfer_current_seq = -1; } else if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); 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); } _mavlink->send_statustext_info("WP CMD OK TRY AGAIN"); } else { if (_verbose) { warnx("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); } _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy"); return; } send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } }
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); } } }