diff --git a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts index 750b3d0..291e624 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts @@ -14,6 +14,7 @@ # AFTS_ENABLED = 1 -> AFTS MODULE PRESENT AND ENABLED param set-default AFTS_ENABLED 1 +param set-default GFTS_PAIRED 0 param set-default MAV_TYPE 37 diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index 62b51a3..53efc61 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -129,6 +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_val = 1; + param_set(param_find("GFTS_PAIRED"), ¶m_val); + } } } else { @@ -222,7 +227,7 @@ FTSController::update_topics() bool FTSController::update_param() { - + param_get(param_find("GFTS_PAIRED"), &_ground_fts_paired); return 1; } diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index e961119..b4aeb48 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -169,6 +169,7 @@ private: hrt_abstime _last_fts_state_update_time{0}; hrt_abstime _ground_fts_timestamp_last{0}; hrt_abstime _auto_fts_timestamp_last{0}; + int32_t _ground_fts_paired{0}; // UORB topics published main_fts_status_s _main_fts_status_msg{}; diff --git a/src/modules/fts_control/fts_control_params.c b/src/modules/fts_control/fts_control_params.c index 1e8176c..a0cd3bf 100644 --- a/src/modules/fts_control/fts_control_params.c +++ b/src/modules/fts_control/fts_control_params.c @@ -46,3 +46,4 @@ */ PARAM_DEFINE_INT32(AFTS_ENABLED, 1); +PARAM_DEFINE_INT32(GFTS_PAIRED, 0);