AFTS arming conditional to GPS lock TBT

boards/px4/fts-auto/init/rc.board_defaults

@@ -17,6 +17,7 @@ param set-default SENS_IMU_MODE 0
 # MAV_0 - TELEM 1 - TTYS5 - UART7 (PE7/PE8)
 param set-default MAV_0_CONFIG 101
 param set-default MAV_0_FORWARD 0
-param set-default MAV_0_MODE 1
+# MODE 15 AFTS TO MFTS
+param set-default MAV_0_MODE 15
 param set-default MAV_0_RADIO_CTL 0
 param set-default MAV_0_RATE 2400

boards/px4/fts-can/init/rc.board_defaults

@@ -17,7 +17,8 @@ param set-default SENS_IMU_MODE 0
 # MAV_0 - TELEM 1 - TTYS5 - UART7 (PE7/PE8)
 param set-default MAV_0_CONFIG 101
 param set-default MAV_0_FORWARD 0
-param set-default MAV_0_MODE 1
+# MODE 13 MFTS TO FC
+param set-default MAV_0_MODE 13
 param set-default MAV_0_RADIO_CTL 0
 param set-default MAV_0_RATE 2400
 
@@ -25,7 +26,8 @@ param set-default MAV_0_RATE 2400
 # MAV_1 - TELEM 2 - TTYS3 - UART5 (PD2/PC12)
 param set-default MAV_1_CONFIG 102
 param set-default MAV_1_FORWARD 0
-param set-default MAV_1_MODE 1
+# MODE 14 MFTS TO GFTS
+param set-default MAV_1_MODE 14
 param set-default MAV_1_RADIO_CTL 0
 param set-default MAV_1_RATE 2400
 

src/modules/fts_control/fts_control.cpp

@@ -57,9 +57,18 @@ FTSController::Run()
     // Update AUTO-FTS status
     if(_auto_fts_message_sub.updated()){
         _auto_fts_message_sub.copy(&_auto_fts_message);
-        if (_auto_fts_message.geofence_violated!=0){
-            fire();
+
+        if (_auto_fts_message.gps_lock!=0){
+            arm();
+
+            if (_auto_fts_message.geofence_violated!=0){
+                fire();
+            }else{
+                reset();
+            }
+
         }else{
+            disarm();
             reset();
         }
     }

src/modules/mavlink/mavlink_main.cpp

@@ -1690,7 +1690,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 	/* fallthrough */
 	case MAVLINK_MODE_CUSTOM:
-        configure_stream_local("AUTO_FTS_MESSAGE", 3.0f);
 		break;
 
 	case MAVLINK_MODE_CONFIG: // USB
@@ -1839,8 +1838,19 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("UAVIONIX_ADSB_OUT_CFG", 0.1f);
 		configure_stream_local("UAVIONIX_ADSB_OUT_DYNAMIC", 5.0f);
 		break;
+    // MODE 13 MFTS TO FC
+    case MAVLINK_MODE_MFTS_FC:
+        configure_stream_local("MAIN_FTS_MESSAGE", 3.0f);
+        break;
+    //MODE 14 MFTS TO GFTS
+    case MAVLINK_MODE_MFTS_GFTS:
+        break;
+    //MODE 15 AFTS TO MFTS
+    case MAVLINK_MODE_AFTS_MFTS:
+        configure_stream_local("AUTO_FTS_MESSAGE", 3.0f);
+        break;
 
-	default:
+        default:
 		ret = -1;
 		break;
 	}

src/modules/mavlink/mavlink_main.h

@@ -214,6 +214,9 @@ public:
 		MAVLINK_MODE_GIMBAL,
 		MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH,
 		MAVLINK_MODE_UAVIONIX,
+        MAVLINK_MODE_MFTS_FC,
+        MAVLINK_MODE_MFTS_GFTS,
+        MAVLINK_MODE_AFTS_MFTS,
 		MAVLINK_MODE_COUNT
 	};
 

src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp

@@ -9,7 +9,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_selector_status.h>
-#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/position_controller_status.h>
@@ -38,6 +38,7 @@ public:
 	uint16_t get_id() override { return get_id_static(); }
 
     uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
+    uORB::Subscription _estimator_gps_status_sub{ORB_ID(estimator_gps_status)};
 
     hrt_abstime _last_reset_time{0};
     hrt_abstime _last_update_time{0};
@@ -65,6 +66,7 @@ private:
 
 			write_geofence_result(&msg);
 
+            write_gps_status(&msg);
 
             mavlink_msg_auto_fts_message_send_struct(_mavlink->get_channel(), &msg);
 
@@ -92,6 +94,22 @@ private:
 		return false;
 	}
 
+    bool write_gps_status(mavlink_auto_fts_message_t *msg)
+    {
+        estimator_gps_status_s gps_status;
+
+        if (_estimator_gps_status_sub.update(&gps_status)) {
+            if (gps_status.checks_passed) {
+                msg->gps_lock = 1;
+            }else {
+                msg->gps_lock = 0;
+            }
+
+            return true;
+        }
+
+        return false;
+    }
 
 	void set_default_values(mavlink_auto_fts_message_t &msg) const
 	{