diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index 53efc61..b9486c1 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/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"), ¶m_val)==0){ + param_get(param_find("GFTS_PAIRED"), ¶m_val); + if (param_val==0){ param_val = 1; param_set(param_find("GFTS_PAIRED"), ¶m_val); } diff --git a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp index e39da74..ebc751f 100644 --- a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp +++ b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include @@ -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; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f131130..bc49ca3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/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"), ¶m_stored); + if (param_new!=param_stored){ + param_set(param_find("GF_LOADED"), ¶m_new); + } } diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 053a0aa..039e3f9 100644 --- a/src/modules/navigator/geofence_params.c +++ b/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);