From b4c38c57767274a79f124761989c2e7a735e12d0 Mon Sep 17 00:00:00 2001 From: neo-nb3 Date: Tue, 11 Jun 2024 19:54:20 +0200 Subject: [PATCH] AFTS arming conditional to GPS lock TBT --- boards/px4/fts-auto/init/rc.board_defaults | 3 ++- boards/px4/fts-can/init/rc.board_defaults | 6 ++++-- src/modules/fts_control/fts_control.cpp | 13 ++++++++++-- src/modules/mavlink/mavlink_main.cpp | 14 +++++++++++-- src/modules/mavlink/mavlink_main.h | 3 +++ .../mavlink/streams/AUTO_FTS_MESSAGE.hpp | 20 ++++++++++++++++++- 6 files changed, 51 insertions(+), 8 deletions(-) diff --git a/boards/px4/fts-auto/init/rc.board_defaults b/boards/px4/fts-auto/init/rc.board_defaults index 5f053f6..26f410d 100644 --- a/boards/px4/fts-auto/init/rc.board_defaults +++ b/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 \ No newline at end of file diff --git a/boards/px4/fts-can/init/rc.board_defaults b/boards/px4/fts-can/init/rc.board_defaults index a7aa758..5cff62d 100644 --- a/boards/px4/fts-can/init/rc.board_defaults +++ b/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 diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index 0fa29ca..b5f3d3c 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/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(); } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d399cf3..8b75786 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c92028c..f1abaea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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 }; diff --git a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp index b5e44d2..ab3fafa 100644 --- a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp +++ b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -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 {