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