|
|
|
@ -305,19 +305,19 @@ void
@@ -305,19 +305,19 @@ void
|
|
|
|
|
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) |
|
|
|
|
{ |
|
|
|
|
mission_item_s mission_item = {}; |
|
|
|
|
bool read_success = false; |
|
|
|
|
int read_result = 0; |
|
|
|
|
|
|
|
|
|
switch (_mission_type) { |
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_MISSION: { |
|
|
|
|
read_success = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s); |
|
|
|
|
read_result = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
|
|
|
|
|
mission_fence_point_s mission_fence_point; |
|
|
|
|
read_success = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) == |
|
|
|
|
sizeof(mission_fence_point_s); |
|
|
|
|
read_result = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) == |
|
|
|
|
sizeof(mission_fence_point_s); |
|
|
|
|
|
|
|
|
|
mission_item.nav_cmd = mission_fence_point.nav_cmd; |
|
|
|
|
mission_item.frame = mission_fence_point.frame; |
|
|
|
@ -337,8 +337,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
@@ -337,8 +337,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
|
|
|
|
|
mission_safe_point_s mission_safe_point; |
|
|
|
|
read_success = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) == |
|
|
|
|
sizeof(mission_safe_point_s); |
|
|
|
|
read_result = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) == |
|
|
|
|
sizeof(mission_safe_point_s); |
|
|
|
|
|
|
|
|
|
mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; |
|
|
|
|
mission_item.frame = mission_safe_point.frame; |
|
|
|
@ -353,7 +353,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
@@ -353,7 +353,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (read_success) { |
|
|
|
|
if (read_result > 0) { |
|
|
|
|
_time_last_sent = hrt_absolute_time(); |
|
|
|
|
|
|
|
|
|
if (_int_mode) { |
|
|
|
|