mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 05:21:35 +08:00
mavlink_mission: guard incoming misison item to be from the current transfer partner
This commit is contained in:
Konrad
@@ -1062,241 +1062,249 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||||||
mavlink_msg_mission_item_decode(msg, &wp);
|
mavlink_msg_mission_item_decode(msg, &wp);
|
||||||
|
|
||||||
if (CHECK_SYSID_COMPID_MISSION(wp)) {
|
if (CHECK_SYSID_COMPID_MISSION(wp)) {
|
||||||
|
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
|
||||||
if (wp.mission_type != _mission_type) {
|
if (wp.mission_type != _mission_type) {
|
||||||
PX4_WARN("WPM: Unexpected mission type (%d %d)", (int)wp.mission_type, (int)_mission_type);
|
PX4_WARN("WPM: Unexpected mission type (%d %d)", (int)wp.mission_type, (int)_mission_type);
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_state == MAVLINK_WPM_STATE_GETLIST) {
|
|
||||||
_time_last_recv = hrt_absolute_time();
|
|
||||||
|
|
||||||
if (wp.seq != _transfer_seq) {
|
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq);
|
|
||||||
|
|
||||||
/* Item sequence not expected, ignore item */
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_state == MAVLINK_WPM_STATE_IDLE) {
|
if (_state == MAVLINK_WPM_STATE_GETLIST) {
|
||||||
if (_transfer_seq == wp.seq + 1) {
|
_time_last_recv = hrt_absolute_time();
|
||||||
// Assume this is a duplicate, where we already successfully got all mission items,
|
|
||||||
// but the GCS did not receive the last ack and sent the same item again
|
if (wp.seq != _transfer_seq) {
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
|
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq);
|
||||||
|
|
||||||
|
/* Item sequence not expected, ignore item */
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (_state == MAVLINK_WPM_STATE_IDLE) {
|
||||||
|
if (_transfer_seq == wp.seq + 1) {
|
||||||
|
// Assume this is a duplicate, where we already successfully got all mission items,
|
||||||
|
// but the GCS did not receive the last ack and sent the same item again
|
||||||
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer");
|
||||||
|
|
||||||
|
_mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t");
|
||||||
|
events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error,
|
||||||
|
"Ignoring mission item, no transfer in progress");
|
||||||
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||||
|
}
|
||||||
|
|
||||||
|
return;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer");
|
PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state);
|
||||||
|
|
||||||
_mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t");
|
_mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t");
|
||||||
events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error,
|
events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error,
|
||||||
"Ignoring mission item, no transfer in progress");
|
"Ignoring mission item, busy");
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
return;
|
struct mission_item_s mission_item = {};
|
||||||
|
|
||||||
} else {
|
int ret = parse_mavlink_mission_item(&wp, &mission_item);
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state);
|
|
||||||
|
|
||||||
_mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t");
|
if (ret != PX4_OK) {
|
||||||
events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error,
|
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
|
||||||
"Ignoring mission item, busy");
|
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct mission_item_s mission_item = {};
|
_mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t");
|
||||||
|
events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error,
|
||||||
|
"Ignoring mission item, invalid item");
|
||||||
|
|
||||||
int ret = parse_mavlink_mission_item(&wp, &mission_item);
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
|
||||||
|
switch_to_idle_state();
|
||||||
if (ret != PX4_OK) {
|
_transfer_in_progress = false;
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
|
return;
|
||||||
|
|
||||||
_mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t");
|
|
||||||
events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error,
|
|
||||||
"Ignoring mission item, invalid item");
|
|
||||||
|
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
|
|
||||||
switch_to_idle_state();
|
|
||||||
_transfer_in_progress = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
_transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32);
|
|
||||||
|
|
||||||
bool write_failed = false;
|
|
||||||
bool check_failed = false;
|
|
||||||
|
|
||||||
switch (_mission_type) {
|
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_MISSION: {
|
|
||||||
// check that we don't get a wrong item (hardening against wrong client implementations, the list here
|
|
||||||
// does not need to be complete)
|
|
||||||
if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
|
|
||||||
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION ||
|
|
||||||
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION ||
|
|
||||||
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION ||
|
|
||||||
mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) {
|
|
||||||
check_failed = true;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
|
|
||||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
|
||||||
reinterpret_cast<uint8_t *>(&mission_item),
|
|
||||||
sizeof(struct mission_item_s));
|
|
||||||
|
|
||||||
// Check for land start marker
|
|
||||||
if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) {
|
|
||||||
_transfer_land_start_marker = wp.seq;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check for land index
|
|
||||||
if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND))
|
|
||||||
&& (_transfer_land_marker == -1)) {
|
|
||||||
_transfer_land_marker = wp.seq;
|
|
||||||
|
|
||||||
if (_transfer_land_start_marker == -1) {
|
|
||||||
_transfer_land_start_marker = _transfer_land_marker;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!write_failed) {
|
|
||||||
/* waypoint marked as current */
|
|
||||||
if (wp.current) {
|
|
||||||
_transfer_current_seq = wp.seq;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_FENCE: { // Write a geofence point
|
|
||||||
mission_fence_point_s mission_fence_point;
|
|
||||||
mission_fence_point.nav_cmd = mission_item.nav_cmd;
|
|
||||||
mission_fence_point.lat = mission_item.lat;
|
|
||||||
mission_fence_point.lon = mission_item.lon;
|
|
||||||
mission_fence_point.alt = mission_item.altitude;
|
|
||||||
|
|
||||||
if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
|
|
||||||
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
|
|
||||||
mission_fence_point.vertex_count = mission_item.vertex_count;
|
|
||||||
|
|
||||||
if (mission_item.vertex_count < 3) { // feasibility check
|
|
||||||
PX4_ERR("Fence: too few vertices");
|
|
||||||
check_failed = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
mission_fence_point.circle_radius = mission_item.circle_radius;
|
|
||||||
}
|
|
||||||
|
|
||||||
mission_fence_point.frame = mission_item.frame;
|
|
||||||
|
|
||||||
if (!check_failed) {
|
|
||||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
|
||||||
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
|
|
||||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
|
||||||
reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
_mavlink.send_statustext_critical("Received unknown mission type, abort.\t");
|
|
||||||
events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error,
|
|
||||||
"Received unknown mission type, abort");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (write_failed || check_failed) {
|
|
||||||
PX4_DEBUG("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);
|
|
||||||
|
|
||||||
if (write_failed) {
|
|
||||||
_mavlink.send_statustext_critical("Unable to write on micro SD\t");
|
|
||||||
events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error,
|
|
||||||
"Mission: unable to write to storage");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
switch_to_idle_state();
|
_transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32);
|
||||||
_transfer_in_progress = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* waypoint marked as current */
|
bool write_failed = false;
|
||||||
if (wp.current) {
|
bool check_failed = false;
|
||||||
_transfer_current_seq = wp.seq;
|
|
||||||
}
|
|
||||||
|
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq);
|
|
||||||
|
|
||||||
_transfer_seq = wp.seq + 1;
|
|
||||||
|
|
||||||
if (_transfer_seq == _transfer_count) {
|
|
||||||
/* got all new mission items successfully */
|
|
||||||
PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%ld, changing state to MAVLINK_WPM_STATE_IDLE",
|
|
||||||
_transfer_count, _transfer_current_seq);
|
|
||||||
|
|
||||||
ret = 0;
|
|
||||||
|
|
||||||
switch (_mission_type) {
|
switch (_mission_type) {
|
||||||
case MAV_MISSION_TYPE_MISSION:
|
|
||||||
_land_start_marker = _transfer_land_start_marker;
|
|
||||||
_land_marker = _transfer_land_marker;
|
|
||||||
|
|
||||||
// Only need to update if the mission actually changed
|
case MAV_MISSION_TYPE_MISSION: {
|
||||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) {
|
// check that we don't get a wrong item (hardening against wrong client implementations, the list here
|
||||||
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
|
// does not need to be complete)
|
||||||
|
if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
|
||||||
|
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION ||
|
||||||
|
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION ||
|
||||||
|
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION ||
|
||||||
|
mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) {
|
||||||
|
check_failed = true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
|
||||||
|
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||||
|
reinterpret_cast<uint8_t *>(&mission_item),
|
||||||
|
sizeof(struct mission_item_s));
|
||||||
|
|
||||||
|
// Check for land start marker
|
||||||
|
if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) {
|
||||||
|
_transfer_land_start_marker = wp.seq;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check for land index
|
||||||
|
if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND))
|
||||||
|
&& (_transfer_land_marker == -1)) {
|
||||||
|
_transfer_land_marker = wp.seq;
|
||||||
|
|
||||||
|
if (_transfer_land_start_marker == -1) {
|
||||||
|
_transfer_land_start_marker = _transfer_land_marker;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!write_failed) {
|
||||||
|
/* waypoint marked as current */
|
||||||
|
if (wp.current) {
|
||||||
|
_transfer_current_seq = wp.seq;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_FENCE:
|
case MAV_MISSION_TYPE_FENCE: { // Write a geofence point
|
||||||
|
mission_fence_point_s mission_fence_point;
|
||||||
|
mission_fence_point.nav_cmd = mission_item.nav_cmd;
|
||||||
|
mission_fence_point.lat = mission_item.lat;
|
||||||
|
mission_fence_point.lon = mission_item.lon;
|
||||||
|
mission_fence_point.alt = mission_item.altitude;
|
||||||
|
|
||||||
|
if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
|
||||||
|
mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
|
||||||
|
mission_fence_point.vertex_count = mission_item.vertex_count;
|
||||||
|
|
||||||
|
if (mission_item.vertex_count < 3) { // feasibility check
|
||||||
|
PX4_ERR("Fence: too few vertices");
|
||||||
|
check_failed = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
mission_fence_point.circle_radius = mission_item.circle_radius;
|
||||||
|
}
|
||||||
|
|
||||||
|
mission_fence_point.frame = mission_item.frame;
|
||||||
|
|
||||||
|
if (!check_failed) {
|
||||||
|
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||||
|
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
|
||||||
|
}
|
||||||
|
|
||||||
// Only need to update if the mission actually changed
|
|
||||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) {
|
|
||||||
ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_RALLY:
|
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
|
||||||
|
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||||
// Only need to update if the mission actually changed
|
reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s);
|
||||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) {
|
|
||||||
ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
PX4_ERR("mission type %u not handled", _mission_type);
|
_mavlink.send_statustext_critical("Received unknown mission type, abort.\t");
|
||||||
|
events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error,
|
||||||
|
"Received unknown mission type, abort");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order
|
if (write_failed || check_failed) {
|
||||||
switch_to_idle_state();
|
PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id);
|
||||||
|
|
||||||
|
|
||||||
if (ret == PX4_OK) {
|
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||||
|
|
||||||
|
if (write_failed) {
|
||||||
|
_mavlink.send_statustext_critical("Unable to write on micro SD\t");
|
||||||
|
events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error,
|
||||||
|
"Mission: unable to write to storage");
|
||||||
|
}
|
||||||
|
|
||||||
|
switch_to_idle_state();
|
||||||
|
_transfer_in_progress = false;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
_transfer_in_progress = false;
|
/* waypoint marked as current */
|
||||||
|
if (wp.current) {
|
||||||
|
_transfer_current_seq = wp.seq;
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq);
|
||||||
|
|
||||||
|
_transfer_seq = wp.seq + 1;
|
||||||
|
|
||||||
|
if (_transfer_seq == _transfer_count) {
|
||||||
|
/* got all new mission items successfully */
|
||||||
|
PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%ld, changing state to MAVLINK_WPM_STATE_IDLE",
|
||||||
|
_transfer_count, _transfer_current_seq);
|
||||||
|
|
||||||
|
ret = 0;
|
||||||
|
|
||||||
|
switch (_mission_type) {
|
||||||
|
case MAV_MISSION_TYPE_MISSION:
|
||||||
|
_land_start_marker = _transfer_land_start_marker;
|
||||||
|
_land_marker = _transfer_land_marker;
|
||||||
|
|
||||||
|
// Only need to update if the mission actually changed
|
||||||
|
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) {
|
||||||
|
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_MISSION_TYPE_FENCE:
|
||||||
|
|
||||||
|
// Only need to update if the mission actually changed
|
||||||
|
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) {
|
||||||
|
ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MAV_MISSION_TYPE_RALLY:
|
||||||
|
|
||||||
|
// Only need to update if the mission actually changed
|
||||||
|
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) {
|
||||||
|
ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
PX4_ERR("mission type %u not handled", _mission_type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order
|
||||||
|
switch_to_idle_state();
|
||||||
|
|
||||||
|
|
||||||
|
if (ret == PX4_OK) {
|
||||||
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
|
||||||
|
|
||||||
|
} 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);
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* request next item */
|
_mavlink.send_statustext_critical("REJ. MIS ITEM: partner id mismatch\t");
|
||||||
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
|
events::send(events::ID("mavlink_mission_item_partner_id_mismatch"), events::Log::Error,
|
||||||
|
"Rejecting mission item, component or system ID mismatch");
|
||||||
|
|
||||||
|
PX4_DEBUG("WPM: MISSION_ITEM ERR: ID mismatch");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user