MAVLink mission transfer state machine

.gitignore

@@ -7,6 +7,7 @@
 *.bak
 *.uvgui.*
 *.ubx
+*.log
 .project
 .settings
 .cproject

src/main/CMakeLists.txt

@@ -412,6 +412,8 @@ main_sources(COMMON_SRC
     mavlink/mavlink_guided.c
     mavlink/mavlink_guided.h
     mavlink/mavlink_internal.h
+    mavlink/mavlink_mission.c
+    mavlink/mavlink_mission.h
     mavlink/mavlink_modes.c
     mavlink/mavlink_modes.h
     mavlink/mavlink_types.h

src/main/fc/fc_mavlink.c

@@ -4,6 +4,7 @@
 
 #include "mavlink/mavlink_command.h"
 #include "mavlink/mavlink_guided.h"
+#include "mavlink/mavlink_mission.h"
 #include "mavlink/mavlink_runtime.h"
 #include "mavlink/mavlink_streams.h"
 
@@ -249,6 +250,22 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn
         return mavlinkHandleIncomingTimesync() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
         return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+        return mavlinkHandleIncomingMissionClearAll() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_COUNT:
+        return mavlinkHandleIncomingMissionCount() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ITEM:
+        return mavlinkHandleIncomingMissionItem() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
+        return mavlinkHandleIncomingMissionItemInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+        return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST:
+        return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
+        return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
+    case MAVLINK_MSG_ID_MISSION_ACK:
+        return mavlinkHandleIncomingMissionAck() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_COMMAND_LONG:
         return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED;
     case MAVLINK_MSG_ID_COMMAND_INT:

src/main/mavlink/mavlink_guided.h

@@ -3,16 +3,11 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "mavlink/mavlink_types.h"
-
-typedef enum {
-    MAV_FRAME_SUPPORTED_NONE = 0,
-    MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0),
-    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT = (1 << 1),
-    MAV_FRAME_SUPPORTED_GLOBAL_INT = (1 << 2),
-    MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT = (1 << 3),
-} mavFrameSupportMask_e;
+#include "mavlink/mavlink_mission.h"
 
 bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask);
 bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame);
 MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters);
+
+bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void);
+bool mavlinkHandleIncomingSetPositionTargetLocalNed(void);

src/main/mavlink/mavlink_internal.h

@@ -74,6 +74,10 @@
 
 #include "scheduler/scheduler.h"
 
+#define MAVLINK_MISSION_UPLOAD_RETRY_MS 1500
+#define MAVLINK_MISSION_UPLOAD_MAX_RETRIES 5
+#define MAVLINK_MISSION_DOWNLOAD_TIMEOUT_MS 5000
+#define MAVLINK_MISSION_CURRENT_INTERVAL_MS 1000
 #define MAVLINK_RX_BYTE_BUDGET 64
 
 typedef struct mavlinkContext_s {
@@ -90,6 +94,10 @@ typedef struct mavlinkContext_s {
     uint8_t autopilotType;
     uint8_t componentId;
     uint8_t recvPortIndex;
+    mavlinkMissionTransfer_t missionTransfer;
+    timeMs_t lastMissionCurrentMs;
+    bool missionCompleted;
+    bool lastWpModeActive;
     uint32_t lastArmingDisableFlags;
     flightModeForTelemetry_e lastFlightMode;
     bool lastGCSNavMode;
@@ -110,5 +118,6 @@ extern mavlinkContext_t mavlinkContext;
 #define mavAutopilotType (mavlinkContext.autopilotType)
 #define mavComponentId (mavlinkContext.componentId)
 #define mavRecvPortIndex (mavlinkContext.recvPortIndex)
+#define mavMissionTransfer (mavlinkContext.missionTransfer)
 
 #endif