From b2c4175b11660ae751184dcef45427a45b1299c5 Mon Sep 17 00:00:00 2001 From: neo Date: Wed, 12 Jun 2024 17:37:25 +0200 Subject: [PATCH] AFTS arming contitional to GPS lock OK --- ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts | 10 +++++++++- boards/px4/fts-auto/nuttx-config/nsh/defconfig | 2 +- src/modules/fts_control/fts_control.cpp | 12 ++++++++++++ src/modules/fts_control/fts_control.hpp | 2 ++ src/modules/mavlink/mavlink_events.cpp | 13 ++++++++----- src/modules/mavlink/mavlink_receiver.cpp | 3 +++ src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp | 10 +++++++--- 7 files changed, 42 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts index a5eefae..6925fc9 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts @@ -22,10 +22,18 @@ # param set-default FTS_SIM_MODE 0 param set-default MAV_TYPE 37 -param set-default GF_ACTION 0 +param set-default GF_ACTION 1 param set-default GF_ALTMODE 0 param set-default GF_COUNT -1 param set-default GF_MAX_HOR_DIST 0.0000 param set-default GF_MAX_VER_DIST 0.0000 param set-default GF_PREDICT 0 param set-default GF_SOURCE 1 + +########################## GPS FAILSAFE REQUIREMENTS ###################################### + +# REQUIRED PRECISION ON GPS POSITION ESTIMATION, METERS +param set-default EKF2_REQ_EPH 10 +param set-default EKF2_REQ_EPV 10 +# REQUIRED NUMBER OF SATELLITES +param set-default EKF2_REQ_NSAT 5 diff --git a/boards/px4/fts-auto/nuttx-config/nsh/defconfig b/boards/px4/fts-auto/nuttx-config/nsh/defconfig index c015083..d12a143 100644 --- a/boards/px4/fts-auto/nuttx-config/nsh/defconfig +++ b/boards/px4/fts-auto/nuttx-config/nsh/defconfig @@ -75,7 +75,7 @@ CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0038 -CONFIG_CDCACM_PRODUCTSTR="FTS-CAN" +CONFIG_CDCACM_PRODUCTSTR="FTS-AUTO" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x3185 diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index b5f3d3c..dd66a50 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -145,6 +145,18 @@ FTSController::reset() fts_drv.fts_off(); } +void +FTSController::arm() +{ + fts_drv.fts_arm(); +} + +void +FTSController::disarm() +{ + fts_drv.fts_disarm(); +} + int FTSController::custom_command(int argc, char *argv[]) { diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index 577ab0d..f7c47b2 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -50,6 +50,8 @@ private: bool update_param(); void fire(); void reset(); + void arm(); + void disarm(); perf_counter_t _cycle_perf; perf_counter_t _interval_perf; diff --git a/src/modules/mavlink/mavlink_events.cpp b/src/modules/mavlink/mavlink_events.cpp index c159653..5cfbb0b 100644 --- a/src/modules/mavlink/mavlink_events.cpp +++ b/src/modules/mavlink/mavlink_events.cpp @@ -233,11 +233,14 @@ void SendProtocol::send_current_sequence(const hrt_abstime &now, bool force_rese return; } - _last_current_sequence_sent = now; - mavlink_current_event_sequence_t current_event_seq; - current_event_seq.sequence = _buffer.get_latest_sequence(); - current_event_seq.flags = (_buffer.size() == 0 || force_reset) ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0; - mavlink_msg_current_event_sequence_send_struct(_mavlink.get_channel(), ¤t_event_seq); + //MOD GG + + //_last_current_sequence_sent = now; + //mavlink_current_event_sequence_t current_event_seq; + //current_event_seq.sequence = _buffer.get_latest_sequence(); + //current_event_seq.flags = (_buffer.size() == 0 || force_reset) ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0; + //MOD + //mavlink_msg_current_event_sequence_send_struct(_mavlink.get_channel(), ¤t_event_seq); } } /* namespace events */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 02e7a8c..0cb37d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3071,6 +3071,9 @@ mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg); auto_fts_message_s auto_fts_message{}; auto_fts_message.timestamp = hrt_absolute_time(); +auto_fts_message.config_status=auto_fts_message_msg.config_status; +auto_fts_message.geofence_loaded=auto_fts_message_msg.geofence_loaded; +auto_fts_message.gps_lock=auto_fts_message_msg.gps_lock; auto_fts_message.geofence_violated=auto_fts_message_msg.geofence_violated; _auto_fts_message_pub.publish(auto_fts_message); } diff --git a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp index ab3fafa..bb17cdb 100644 --- a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp +++ b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp @@ -99,15 +99,19 @@ private: estimator_gps_status_s gps_status; if (_estimator_gps_status_sub.update(&gps_status)) { - if (gps_status.checks_passed) { + //FAIL FLAGS THAT NEED TO BE CLEAR TO INDICATE GPS OK + if ( + !gps_status.check_fail_gps_fix && + !gps_status.check_fail_min_sat_count && + !gps_status.check_fail_max_horz_err && + !gps_status.check_fail_max_vert_err + ) { msg->gps_lock = 1; }else { msg->gps_lock = 0; } - return true; } - return false; }