ground FTS pairing WIP

boards/px4/fts-can/init/rc.board_defaults

@@ -10,7 +10,7 @@ param set-default SYS_AUTOSTART 1
 param set-default MAV_PROTO_VER 2
 
 # Log from boot until shutdown
-param set-default SDLOG_MODE 2
+#param set-default SDLOG_MODE 2
 
 param set-default SYS_USE_IO 0
 param set-default SENS_IMU_MODE 0

boards/px4/fts-can/init/rc.board_sensors

@@ -2,5 +2,4 @@
 #
 # PX4 FTS-CAN specific board sensors init
 #------------------------------------------------------------------------------
-board_adc start
+board_adc start
-fts_control start

msg/CMakeLists.txt

@@ -68,6 +68,7 @@ set(msg_files
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	Ekf2Timestamps.msg
+	EmergencyCommand.msg
 	EscReport.msg
 	EscStatus.msg
 	EstimatorAidSource1d.msg
@@ -103,6 +104,8 @@ set(msg_files
 	GpsInjectData.msg
 	GimbalControls.msg
 	Gripper.msg
+	GroundFtsMessage.msg
+	GroundFtsResponse.msg
 	HealthReport.msg
 	HeaterStatus.msg
 	HomePosition.msg

msg/EmergencyCommand.msg

@@ -0,0 +1,6 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 target_system
+uint8 target_component
+uint8 emergency_command
+uint32 param

msg/GroundFtsMessage.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 message
+uint32 fts_uid

msg/GroundFtsResponse.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 message
+uint32 fts_uid

src/modules/fts_control/fts_control.cpp

@@ -1,11 +1,9 @@
-/**
- *
- * Controller of Nimbus Charging FTS
- *
- * @author Flavio Tonelli <f.tonelli@nimbus.aero>
- */
 
 #include "fts_control.hpp"
+#include "px4_platform_common/board_common.h"
+
+
+#include "../mavlink/streams/GROUND_FTS_RESPONSE.hpp"
 
 FTSController::FTSController() :
 	ModuleParams(nullptr),
@@ -30,6 +28,12 @@ FTSController::init()
 		PX4_WARN("fts_control failed to get necessary parameters.");
 		return false;
 	}
+    uint8_t guid[PX4_GUID_BYTE_LENGTH];
+    board_get_px4_guid(guid);
+    fts_id.uidBytes[0]=guid[0];
+    fts_id.uidBytes[1]=guid[1];
+    fts_id.uidBytes[2]=guid[2];
+    fts_id.uidBytes[3]=guid[3];
 
 	// run every 1s
 	ScheduleOnInterval(1000_ms);
@@ -73,7 +77,27 @@ FTSController::Run()
         }
     }
 
-	// Publish FTS controller state
+    // Update GROUND-FTS status
+    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){
+
+                _ground_fts_response_pub.publish(_ground_fts_response);
+
+            }
+        }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);
+            MavlinkStreamGroundftsResponse.send();
+        }
+    }
+    // Publish FTS controller state
 	_fts_state.timestamp = hrt_absolute_time();
 
 	perf_end(_cycle_perf);
@@ -170,6 +194,16 @@ FTSController::custom_command(int argc, char *argv[])
         return 0;
     }
 
+    if (!strcmp(argv[0], "arm")) {
+        get_instance()->arm();
+        return 0;
+    }
+
+    if (!strcmp(argv[0], "disarm")) {
+        get_instance()->disarm();
+        return 0;
+    }
+
     return print_usage("unknown command");
 }
 
@@ -201,6 +235,8 @@ FTSController::print_usage(const char *reason)
 	PRINT_MODULE_USAGE_NAME("fts_controller", "system");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_COMMAND("status");
+    PRINT_MODULE_USAGE_COMMAND("arm");
+    PRINT_MODULE_USAGE_COMMAND("disarm");
     PRINT_MODULE_USAGE_COMMAND("fire");
     PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

src/modules/fts_control/fts_control.hpp

@@ -15,12 +15,33 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/fts_state.h>
 #include <uORB/topics/auto_fts_message.h>
+#include <uORB/topics/ground_fts_message.h>
+#include <uORB/topics/ground_fts_response.h>
+#include <uORB/topics/emergency_command.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <drivers/fts/fts.h>
 using namespace time_literals;
 
+// Helper union to construct the UID field of the Mavlink message.
+union FTSIDUnion{
+    uint32_t uid;
+    uint8_t uidBytes[4];
+};
+
+typedef enum FTS_MESSAGE_TYPE
+{
+    PING=1, /* Send from the host to the FTS. | */
+    PONG=2, /* Send from the FTS to the host. | */
+    PAIR=3, /* A Pairing operation. Send from host to FTS means the host want to start a pairing procedure.
+                    The FTS will ignore the FTS_UID field check when it receives this message and send back this
+                    same message as a response. Then the host shall register the FTS_UID field in the response.
+                 | */
+    FTS_MESSAGE_TYPE_ENUM_END=4, /*  | */
+} FTS_MESSAGE_TYPE;
+
+
 class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -56,18 +77,25 @@ private:
 	perf_counter_t _cycle_perf;
 	perf_counter_t _interval_perf;
 
-	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+    uORB::SubscriptionData<auto_fts_message_s> _auto_fts_message_sub{ORB_ID(auto_fts_message)};
-	uORB::SubscriptionData<auto_fts_message_s> _auto_fts_message_sub{ORB_ID(auto_fts_message)};
+    uORB::SubscriptionData<ground_fts_message_s> _ground_fts_message_sub{ORB_ID(ground_fts_message)};
-	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
+    uORB::SubscriptionData<emergency_command_s> _emergency_command_sub{ORB_ID(emergency_command)};
-	uORB::SubscriptionData<vehicle_local_position_s> _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
     uORB::Publication<fts_state_s>	_fts_state_pub{ORB_ID(fts_state)};
+    uORB::Publication<ground_fts_response_s>	_ground_fts_response_pub{ORB_ID(ground_fts_response)};
 
     FTS fts_drv;
     hrt_abstime _last_fts_state_update_time{0};
 
 	fts_state_s _fts_state{};
     auto_fts_message_s _auto_fts_message{};
+    ground_fts_message_s _ground_fts_message{};
+    ground_fts_response_s _ground_fts_response{};
+    emergency_command_s _emergency_command{};
 
 	uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz
+
+    // Global variable storing the UID of the chip.
+    union FTSIDUnion fts_id;
+
 };

src/modules/mavlink/mavlink_main.cpp

@@ -1844,6 +1844,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
         break;
     //MODE 14 MFTS TO GFTS
     case MAVLINK_MODE_MFTS_GFTS:
+        configure_stream_local("GROUND_FTS_RESPONSE", 0.0f);
         break;
     //MODE 15 AFTS TO MFTS
     case MAVLINK_MODE_AFTS_MFTS:

src/modules/mavlink/mavlink_messages.cpp

@@ -82,6 +82,7 @@
 #include "streams/GPS_RAW_INT.hpp"
 #include "streams/GPS_RTCM_DATA.hpp"
 #include "streams/GPS_STATUS.hpp"
+#include "streams/GROUND_FTS_RESPONSE.hpp"
 #include "streams/HEARTBEAT.hpp"
 #include "streams/HIGHRES_IMU.hpp"
 #include "streams/HIL_ACTUATOR_CONTROLS.hpp"
@@ -485,8 +486,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
 #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
 #if defined(AUTO_FTS_MESSAGE_HPP)
-        create_stream_list_item<MavlinkStreamAftsStatus>()
+        create_stream_list_item<MavlinkStreamAftsStatus>(),
 #endif // AUTO_FTS_MESSAGE_HPP
+#if defined(GROUND_FTS_RESPONSE_HPP)
+        create_stream_list_item<MavlinkStreamGroundftsResponse>()
+#endif // GROUND_FTS_RESPONSE_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/mavlink_receiver.cpp

@@ -316,6 +316,12 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_AUTO_FTS_MESSAGE:
         handle_message_auto_fts_message(msg);
         break;
+    case MAVLINK_MSG_ID_FTS_MESSAGE:
+        handle_message_ground_fts_message(msg);
+        break;
+    case MAVLINK_MSG_ID_EMERGENCY_COMMAND:
+        handle_message_emergency_command(msg);
+        break;
 
         default:
 		break;
@@ -3066,16 +3072,34 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 void
 MavlinkReceiver::handle_message_auto_fts_message(mavlink_message_t *msg)
 {
-mavlink_auto_fts_message_t auto_fts_message_msg;
+    mavlink_auto_fts_message_t auto_fts_message_msg;
-mavlink_msg_auto_fts_message_decode(msg, &auto_fts_message_msg);
+    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);
+}
+
+void
+MavlinkReceiver::handle_message_ground_fts_message(mavlink_message_t *msg)
+{
+    mavlink_fts_message_t ground_fts_message_mavlink;
+    mavlink_msg_fts_message_decode(msg, &ground_fts_message_mavlink);
+
+
+    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_pub.publish(ground_fts_message);
+}
+
+void
+MavlinkReceiver::handle_message_emergency_command(mavlink_message_t *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);
 }
 
 void

src/modules/mavlink/mavlink_receiver.h

@@ -71,6 +71,7 @@
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/emergency_command.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
@@ -84,6 +85,7 @@
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/log_message.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/ground_fts_message.h>
 #include <uORB/topics/mavlink_tunnel.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/offboard_control_mode.h>
@@ -166,6 +168,7 @@ private:
 	void handle_message_command_int(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_distance_sensor(mavlink_message_t *msg);
+    void handle_message_emergency_command(mavlink_message_t *msg);
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_generator_status(mavlink_message_t *msg);
 	void handle_message_set_gps_global_origin(mavlink_message_t *msg);
@@ -178,6 +181,7 @@ private:
 	void handle_message_landing_target(mavlink_message_t *msg);
 	void handle_message_logging_ack(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
+    void handle_message_ground_fts_message(mavlink_message_t *msg);
 	void handle_message_obstacle_distance(mavlink_message_t *msg);
 	void handle_message_odometry(mavlink_message_t *msg);
 	void handle_message_onboard_computer_status(mavlink_message_t *msg);
@@ -296,6 +300,7 @@ private:
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
+    uORB::Publication<emergency_command_s>	_emergency_command_pub{ORB_ID(emergency_command)};
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
 	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<gimbal_manager_set_manual_control_s>	_gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
@@ -304,6 +309,7 @@ private:
 	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
 	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};
+    uORB::Publication<ground_fts_message_s>	_ground_fts_message_pub{ORB_ID(ground_fts_message)};
 	uORB::Publication<mavlink_tunnel_s>			_mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)};
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<offboard_control_mode_s>		_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};

src/modules/mavlink/streams/GROUND_FTS_RESPONSE.hpp

@@ -0,0 +1,70 @@
+#ifndef GROUND_FTS_RESPONSE_HPP
+#define GROUND_FTS_RESPONSE_HPP
+
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/ground_fts_response.h>
+#include <px4_platform_common/events.h>
+
+class MavlinkStreamGroundftsResponse : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGroundftsResponse(mavlink); }
+
+	static constexpr const char *get_name_static() { return "GROUND_FTS_RESPONSE"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FTS_MESSAGE; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+    uORB::Subscription _ground_fts_response_sub{ORB_ID(ground_fts_response)};
+
+    hrt_abstime _last_reset_time{0};
+    hrt_abstime _last_update_time{0};
+
+    unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	bool const_rate() override { return true; }
+
+private:
+	explicit MavlinkStreamGroundftsResponse(Mavlink *mavlink) :
+		MavlinkStream(mavlink)
+	{
+	}
+
+	bool send() override
+	{
+		// only send the struct if transmitting is allowed
+		// this assures that the stream timer is only reset when actually a message is transmitted
+		if (_mavlink->should_transmit()) {
+			mavlink_fts_message_t msg{};
+
+            ground_fts_response_s response;
+
+            if (_ground_fts_response_sub.update(&response)) {
+                msg.message = response.message;
+                msg.FTS_UID = response.fts_uid;
+
+                return true;
+            }
+
+            return false;
+
+            mavlink_msg_fts_message_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+		return false;
+
+	}
+
+};
+
+#endif