AFTS arming contitional to GPS lock OK

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

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

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[])
 {

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;

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;
+    //MOD GG
-	mavlink_current_event_sequence_t current_event_seq;
+
-	current_event_seq.sequence = _buffer.get_latest_sequence();
+	//_last_current_sequence_sent = now;
-	current_event_seq.flags = (_buffer.size() == 0 || force_reset) ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0;
+	//mavlink_current_event_sequence_t current_event_seq;
-	mavlink_msg_current_event_sequence_send_struct(_mavlink.get_channel(), &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(), &current_event_seq);
 }
 
 } /* namespace events */

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);
 }

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;
     }