ground FTS pairing & terminate OK

src/modules/fts_control/fts_control.cpp

@@ -3,7 +3,6 @@
 #include "px4_platform_common/board_common.h"
 
 
-//#include "../mavlink/streams/GROUND_FTS_RESPONSE.hpp"
 
 FTSController::FTSController() :
 	ModuleParams(nullptr),
@@ -41,68 +40,83 @@ FTSController::init()
 }
 
 void
-FTSController::Run()
+FTSController::Run() {
-{
+    if (should_exit()) {
-	if (should_exit()) {
+        ScheduleClear();
-		ScheduleClear();
+        exit_and_cleanup();
-		exit_and_cleanup();
+        return;
-		return;
+    }
-	}
 
-	perf_begin(_cycle_perf);
+    perf_begin(_cycle_perf);
-	perf_count(_interval_perf);
+    perf_count(_interval_perf);
 
-	// Update all subscriber
+    // Update all subscriber
-	update_topics();
+    update_topics();
 
-	// Update FTS comm status
+    // Update FTS comm status
-	update_status();
+    update_status();
 
     // Update AUTO-FTS status
-    if(_auto_fts_message_sub.updated()){
+    if (_auto_fts_message_sub.updated()) {
         _auto_fts_message_sub.copy(&_auto_fts_message);
 
-        if (_auto_fts_message.gps_lock!=0){
+        if (_auto_fts_message.gps_lock != 0) {
             arm();
 
-            if (_auto_fts_message.geofence_violated!=0){
+            if (_auto_fts_message.geofence_violated != 0) {
                 fire();
-            }else{
+            } else {
                 reset();
             }
 
-        }else{
+        } else {
             disarm();
             reset();
         }
     }
 
     // Update GROUND-FTS status
-    if(_ground_fts_message_sub.updated()) {
+    if (_ground_fts_message_sub.updated()) {
 
         _ground_fts_message_sub.copy(&_ground_fts_message);
 
         if (_ground_fts_message.message == PING) {
             //PING COMMAND
-            if(_ground_fts_message.fts_uid == fts_id.uid){
+            if (_ground_fts_message.fts_uid == fts_id.uid) {
 
+                _ground_fts_response.message = PONG;
+                _ground_fts_response.fts_uid = fts_id.uid;
                 _ground_fts_response_pub.publish(_ground_fts_response);
 
             }
-        }else if(_ground_fts_message.message == PAIR){
+        } else if (_ground_fts_message.message == PAIR) {
             //PAIR COMMAND
             _ground_fts_response.message = PAIR;
             _ground_fts_response.fts_uid = fts_id.uid;
             _ground_fts_response_pub.publish(_ground_fts_response);
         }
     }
-    // Publish FTS controller state
-	_fts_state.timestamp = hrt_absolute_time();
 
-	perf_end(_cycle_perf);
+    if (_emergency_command_sub.updated()) {
+
+        _emergency_command_sub.copy(&_emergency_command);
+
+        if (_emergency_command.param == fts_id.uid) {
+            if (_emergency_command.emergency_command == ACTIVATE_FTS) {
+                //TERMINATE COMMAND
+                fire();
+            }
+        }
+
+        // Publish FTS controller state
+        _fts_state.timestamp = hrt_absolute_time();
+
+        perf_end(_cycle_perf);
+    }
 }
 
-void FTSController::update_status()
+void
+FTSController::update_status()
 {
 
 	// Update FTS state

src/modules/fts_control/fts_control.hpp

@@ -41,6 +41,16 @@ typedef enum FTS_MESSAGE_TYPE
     FTS_MESSAGE_TYPE_ENUM_END=4, /*  | */
 } FTS_MESSAGE_TYPE;
 
+typedef enum MAV_EMERGENCY_COMMAND_TYPE
+{
+    ACTIVATE_FTS=1, /* Stop motors and open the parachute | */
+    EMERGENCY_LANDING=2, /* Land wherever as soon as possible | */
+    GO_TO_RALLY_POINT=3, /* Identify the closest rally point to land | */
+    HOLD=4, /* Hold position in multi copter mode | */
+    UNHOLD=5, /* Hold position in multi copter mode | */
+    DISACTIVATE_FTS=6, /* re-enable motors and restore parachute output (testing only) | */
+    MAV_EMERGENCY_COMMAND_TYPE_ENUM_END=7, /*  | */
+} MAV_EMERGENCY_COMMAND_TYPE;
 
 class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
 {

src/modules/mavlink/mavlink_receiver.cpp

@@ -3094,11 +3094,21 @@ MavlinkReceiver::handle_message_ground_fts_message(mavlink_message_t *msg)
     ground_fts_message_s ground_fts_message{};
     ground_fts_message.timestamp = hrt_absolute_time();
     ground_fts_message.message = ground_fts_message_mavlink.message;
+    ground_fts_message.fts_uid = ground_fts_message_mavlink.FTS_UID;
     _ground_fts_message_pub.publish(ground_fts_message);
 }
 
 void
 MavlinkReceiver::handle_message_emergency_command(mavlink_message_t *msg) {
+    mavlink_emergency_command_t emergency_command_mavlink;
+    mavlink_msg_emergency_command_decode(msg, &emergency_command_mavlink);
+
+
+    emergency_command_s emergency_command{};
+    emergency_command.timestamp = hrt_absolute_time();
+    emergency_command.emergency_command = emergency_command_mavlink.emergency_command;
+    emergency_command.param = emergency_command_mavlink.param;
+    _emergency_command_pub.publish(emergency_command);
 
 }
 

src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp

@@ -35,24 +35,23 @@ public:
 private:
 
 	explicit MavlinkStreamGroundFtsResponse(Mavlink *mavlink) :	MavlinkStream(mavlink)	{}
+    mavlink_fts_message_t msg{};
+    ground_fts_response_s response;
 
 	bool send() override
 	{
+        mavlink_msg_fts_message_send_struct(_mavlink->get_channel(), &msg);
+        return true;
+	}
 
-        mavlink_fts_message_t msg{};
+    void update_data() override {
-
-        ground_fts_response_s response;
-
         if (_ground_fts_response_sub.update(&response)) {
             msg.message = response.message;
             msg.FTS_UID = response.fts_uid;
 
-            mavlink_msg_fts_message_send_struct(_mavlink->get_channel(), &msg);
+            send();
-
-            return true;
         }
-        return false;
+    }
-	}
 };
 
 #endif