added auto-fts geofence_loaded check & publish

src/modules/fts_control/fts_control.cpp

@@ -129,8 +129,11 @@ FTSController::Run() {
             _ground_fts_response.message = PAIR;
             _ground_fts_response.fts_uid = fts_id.uid;
             _ground_fts_response_pub.publish(_ground_fts_response);
+
+
             int32_t param_val = 0;
-            if (param_get(param_find("GFTS_PAIRED"), &param_val)==0){
+            param_get(param_find("GFTS_PAIRED"), &param_val);
+            if (param_val==0){
                 param_val = 1;
                 param_set(param_find("GFTS_PAIRED"), &param_val);
             }

src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp

@@ -23,6 +23,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failsafe_flags.h>
 #include <uORB/topics/health_report.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <px4_platform_common/events.h>
 
@@ -42,7 +43,7 @@ public:
 
     hrt_abstime _last_reset_time{0};
     hrt_abstime _last_update_time{0};
-
+    int32_t _gf_loaded{0};
     unsigned get_size() override
 	{
 		return MAVLINK_MSG_ID_AUTO_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -54,13 +55,17 @@ private:
 	explicit MavlinkStreamAftsStatus(Mavlink *mavlink) :
 		MavlinkStream(mavlink)
 	{
+        _param_gf_loaded_uuid = param_find("GF_LOADED");
 	}
+    param_t _param_gf_loaded_uuid{PARAM_INVALID};
 
 	bool send() override
 	{
         mavlink_auto_fts_message_t msg{};
         set_default_values(msg);
 
+        write_config_status(&msg);
+
         write_geofence_result(&msg);
 
         write_gps_status(&msg);
@@ -71,7 +76,17 @@ private:
 
 	}
 
-	bool write_geofence_result(mavlink_auto_fts_message_t *msg)
+    bool write_config_status(mavlink_auto_fts_message_t *msg) {
+        param_get(_param_gf_loaded_uuid, &_gf_loaded);
+        if(_gf_loaded){
+            msg->geofence_loaded = 1;
+        }else{
+            msg->geofence_loaded = 0;
+        }
+        return true;
+    }
+
+    bool write_geofence_result(mavlink_auto_fts_message_t *msg)
 	{
 		geofence_result_s geofence;
 

src/modules/navigator/geofence.cpp

@@ -174,6 +174,14 @@ void Geofence::_updateFence()
 		}
 
 	}
+    //SAVE GEOFENCE LOADED STATUS FOR FTS FAILSAFE
+    int32_t param_stored{0};
+    int32_t param_new{0};
+    param_new = (_num_polygons>0);
+    param_get(param_find("GF_LOADED"), &param_stored);
+    if (param_new!=param_stored){
+        param_set(param_find("GF_LOADED"), &param_new);
+    }
 
 }
 

src/modules/navigator/geofence_params.c

@@ -140,3 +140,14 @@ PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_PREDICT, 0);
+
+/**
+ * Geofence profile loaded flag
+ *
+ * Used to report the AUTO-FTS configuration status
+ * 0 = profile not lodaded, 1 = profile loaded
+ *
+ * @boolean
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_LOADED, 0);