mfts afts receiver dev

ROMFS/px4fmu_common/init.d/airframes/0001_main_fts

@@ -22,3 +22,5 @@ param set-default FTS_TYPE 1
 param set-default FTS_SIM_MODE 0
 
 param set-default MAV_TYPE 37
+
+fts start

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

@@ -5,6 +5,8 @@
 
 param set-default SYS_AUTOSTART 2
 
+param set-default MAV_PROTO_VER 2
+
 #param set-default FTS_TYPE 2
 #param set-default FTS_SIM_MODE 0
 

boards/px4/fts-auto/nuttx-config/Kconfig

@@ -4,7 +4,7 @@
 #
 config BOARD_HAS_PROBES
 	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
-	default y
+	default n
 	---help---
 		This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers.
 

boards/px4/fts-auto/nuttx-config/bootloader/defconfig

@@ -32,11 +32,11 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
 CONFIG_CDCACM_PRODUCTID=0x0038
-CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6C.x"
+CONFIG_CDCACM_PRODUCTSTR="FTS-AUTO"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
-CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CDCACM_VENDORSTR="Nimbus"
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y

boards/px4/fts-auto/nuttx-config/nsh/defconfig

@@ -75,11 +75,11 @@ CONFIG_BUILTIN=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
 CONFIG_CDCACM_PRODUCTID=0x0038
-CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x"
+CONFIG_CDCACM_PRODUCTSTR="FTS-CAN"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
-CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CDCACM_VENDORSTR="Nimbus"
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
 CONFIG_DEBUG_MEMFAULT=y

boards/px4/fts-can/default.px4board

@@ -7,6 +7,7 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_TELEMETRY=y
+CONFIG_MAVLINK_DIALECT="customNimbus"
 CONFIG_MODULES_BATTERY_STATUS=n
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y

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

@@ -5,6 +5,8 @@
 
 param set-default SYS_AUTOSTART 1
 
+param set-default MAV_PROTO_VER 2
+
 param set-default FTS_TYPE 1
 param set-default FTS_SIM_MODE 0
 

msg/AutoFtsMessage.msg

@@ -0,0 +1,6 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 config_status = 0
+uint8 geofence_loaded = 1
+uint8 gps_lock = 2
+uint8 geofence_violated = 3

msg/CMakeLists.txt

@@ -49,6 +49,7 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
+	AutoFtsMessage.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg

src/drivers/fts/fts.h

@@ -63,11 +63,10 @@ private:
 	/** Disables the fts */
 	void disable_fts();
 
-	/** Turns the fts on */
 	void fts_on();
-
-	/** Turns the fts off */
 	void fts_off();
+    void fts_arm();
+    void fts_disarm();
 
 	void initialize();
 

src/modules/fts_control/CMakeLists.txt

@@ -2,6 +2,6 @@ px4_add_module(
 	MODULE modules__fts_control
 	MAIN fts_controller
 	SRCS
-		fts_controller.cpp
+        fts_control.cpp
 		main_state_machine.cpp
 	)

src/modules/fts_control/fts_control.cpp

@@ -5,7 +5,7 @@
  * @author Flavio Tonelli <f.tonelli@nimbus.aero>
  */
 
-#include "fts_controller.hpp"
+#include "fts_control.hpp"
 
 FTSController::FTSController() :
 	ModuleParams(nullptr),
@@ -27,13 +27,13 @@ FTSController::init()
 {
 	// Update parameters
 	if (!update_param()) {
-		PX4_WARN("fts_controller failed to get necessary parameters.");
+		PX4_WARN("fts_control failed to get necessary parameters.");
 		return false;
 	}
 
 	//_main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state);
-	// run every 50 ms (20 Hz)
-	ScheduleOnInterval(50_ms);
+	// run every 1s
+	ScheduleOnInterval(1000_ms);
 	return true;
 }
 
@@ -96,17 +96,12 @@ FTSController::update_topics()
 bool FTSController::update_param()
 {
 
-	// Dummy find all the FTSController params. So the NimbusGUI will read these params on first connection.
 	param_find("FTS_GEOFENCE_ENABLED");
 	param_find("FTS_MANUAL_FTS_ENABLED");
+	param_find("FTS_COMM_TIMEOUT");
 
+	float fts_comm_timeout_s = 5.0f;
 
-	param_t fts_comm_timeout_s_handle = param_find("FTS_COMM_TIMEOUT");
-	float fts_comm_timeout_s = 0.0f;
-
-	if (param_get(fts_comm_timeout_s_handle, &fts_comm_timeout_s) != PX4_OK) {
-		return false;
-	}
 
 	_fts_comm_timeout_threshold_us = (uint64_t)fts_comm_timeout_s * 1000000;
 

src/modules/mavlink/mavlink_main.cpp

@@ -1593,7 +1593,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 	/* fallthrough */
 	case MAVLINK_MODE_CUSTOM:
-        configure_stream_local("AUTO_FTS_MESSAGE", 1.0f);
+        configure_stream_local("AUTO_FTS_MESSAGE", 3.0f);
 		break;
 
 	case MAVLINK_MODE_CONFIG: // USB

src/modules/mavlink/mavlink_receiver.cpp

@@ -313,7 +313,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;
 
-	default:
+    case MAVLINK_MSG_ID_AUTO_FTS_MESSAGE:
+        handle_message_auto_fts_message(msg);
+        break;
+
+        default:
 		break;
 	}
 
@@ -3059,6 +3063,17 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
 }
 
+MavlinkReceiver::handle_message_auto_fts_message(mavlink_message_t *msg)
+{
+mavlink_auto_fts_message_t 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_pub.publish(gimbal_attitude_status);
+}
+
 void
 MavlinkReceiver::run()
 {

src/modules/mavlink/mavlink_receiver.h

@@ -288,6 +288,7 @@ private:
 
 	// ORB publications
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
+    uORB::Publication<auto_fts_message_s>	_auto_fts_message_pub{ORB_ID(auto_fts_message)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};