fts module dev

CMakeLists.txt

@@ -426,6 +426,7 @@ if(EXISTS "${PX4_BOARD_DIR}/CMakeLists.txt")
 endif()
 
 foreach(module ${config_module_list})
+	message(STATUS "Adding module ${module}")
 	add_subdirectory(src/${module})
 endforeach()
 

boards/px4/fts-proto/default.px4board

@@ -10,6 +10,7 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_FTS_CONTROL=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAVLINK=y

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

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

build_mfts.sh

@@ -5,4 +5,4 @@ rm -rf build/
 git submodule update --recursive
 #make -j$(nproc) px4_fts-can
 #make -j$(nproc) px4_fts-proto
-make px4_fts-proto
+make px4_fts-proto_default

cmake/kconfig.cmake

@@ -159,7 +159,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
 
 		# Find variable name
 		string(REGEX MATCH "^CONFIG_MODULES[^=]+" Modules ${NameAndValue})
-
 		if(Modules)
 			# Find the value
 			string(REPLACE "${Name}=" "" Value ${NameAndValue})

msg/CMakeLists.txt

@@ -88,7 +88,7 @@ set(msg_files
 	FollowTarget.msg
 	FollowTargetEstimator.msg
 	FollowTargetStatus.msg
-	FtsStatus.msg
+	FtsState.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GimbalDeviceAttitudeStatus.msg

msg/FtsState.msg

@@ -1,5 +1,8 @@
 uint64 timestamp	# time since system start (microseconds)
 
 bool fts_on
+uint8 ground_fts_state
+uint8 auto_fts_state
+uint8 main_fts_state
 
 

src/drivers/fts/fts.cpp

@@ -33,7 +33,7 @@ FTS::FTS() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
 {
 
-	_fts_status_pub.advertise();
+	_fts_state_pub.advertise();
 }
 
 FTS::~FTS()
@@ -107,10 +107,10 @@ void FTS::Run()
 
 void FTS::publish_status()
 {
-	fts_status_s status{};
+	fts_state_s status{};
 	status.fts_on               = _fts_on;
 	status.timestamp = hrt_absolute_time();
-	_fts_status_pub.publish(status);
+	_fts_state_pub.publish(status);
 }
 
 int FTS::start()

src/drivers/fts/fts.h

@@ -11,7 +11,7 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/fts_status.h>
+#include <uORB/topics/fts_state.h>
 #include <uORB/topics/parameter_update.h>
 
 #include <mathlib/mathlib.h>
@@ -95,7 +95,7 @@ private:
 	bool _fts_initialized     = false;
 	bool _fts_on              = false;
 
-	uORB::Publication<fts_status_s> _fts_status_pub{ORB_ID(fts_status)};
+	uORB::Publication<fts_state_s> _fts_state_pub{ORB_ID(fts_state)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 

src/modules/fts/main_state_machine.cpp

@@ -1,89 +0,0 @@
-#include "main_state_machine.hpp"
-
-MainStateMachine::MainStateMachine()
-{
-}
-
-uint8_t MainStateMachine::get_mainstatemachine_state()
-{
-}
-
-void MainStateMachine::init_state_machine(uORB::Publication<main_fts_state_s> *main_fts_state_pub,
-		MainStateMachine::fts_state_s *FTS_state)
-{
-}
-
-void MainStateMachine::send_cmd_ack(uint8_t cmd_result, uint8_t refuse_reason)
-{
-	if (!_ground_cmd_reply_pub) {
-		return;
-	}
-
-	ground_fts_reply_s msg{};
-	msg.timestamp = hrt_absolute_time();
-	msg.cmd_result = cmd_result;
-
-	_ground_cmd_reply_pub->publish(msg);
-}
-
-bool MainStateMachine::send_fts_cmd(uint8_t cmd)
-{
-	if (!_fts_cmd_pub) {
-		PX4_ERR("FTS command publisher null");
-		return false;
-	}
-
-	// Limit the command send rate
-	if (hrt_elapsed_time(&_cmd_sent_time) < FTS_STATE_SEND_INTERVAL) {
-		return false;
-	}
-
-	fts_control_cmd_s msg{};
-	msg.timestamp = hrt_absolute_time();
-	msg.command = cmd;
-	_cmd_sent_time = msg.timestamp;
-	_fts_cmd_pub->publish(msg);
-
-	return true;
-}
-
-bool MainStateMachine::check_ground_fts_timeout()
-{
-	if (hrt_elapsed_time(&_last_mfts_ping_time) > FTS_PING_TIMEOUT) {
-		return true;
-	}
-	return false;
-}
-bool MainStateMachine::check_auto_fts_timeout()
-{
-    if (hrt_elapsed_time(&_last_afts_ping_time) > FTS_PING_TIMEOUT) {
-        return true;
-    }
-    return false;
-}
-
-bool MainStateMachine::check_fts_error()
-{
-	if (_fts_state->fts_state == fts_state_s::MAIN_STATE_ERROR) {
-		// FTS in an error state. Notice the server.
-		send_cmd_ack(fts_server_cmd_ack_s::SERVER_CMD_REFUSED, fts_server_cmd_ack_s::REFUSE_REASON_FTS_ERROR);
-		go_to_error_state();
-		return true;
-	}
-
-	return false;
-}
-
-void MainStateMachine::run_state_machine()
-{
-	switch (MAIN_STATE_t(_current_node->node_index)) {
-	case IDLE:
-		break;
-
-	case ERROR_STATE:
-		break;
-
-	case END_OF_STATE:
-		break;
-	}
-}

src/modules/fts_control/CMakeLists.txt

@@ -1,9 +1,7 @@
 px4_add_module(
-	MODULE modules__fts
+	MODULE modules__fts_control
 	MAIN fts_controller
 	SRCS
 		fts_controller.cpp
 		main_state_machine.cpp
-	DEPENDS
-		px4_work_queue
 	)

src/modules/fts_control/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_FTS_CONTROL
+	bool "fts_control"
+	default n
+	---help---
+		Enable support for fts_control

src/modules/fts_control/fts_controller.cpp

@@ -31,7 +31,7 @@ FTSController::init()
 		return false;
 	}
 
-	_main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state);
+	//_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);
 	return true;
@@ -55,17 +55,14 @@ FTSController::Run()
 	// Update FTS comm status
 	update_status();
 
-	// Update server command
-	update_server_cmd();
-
 	// Excecute the state machine
 	_main_state_machine.run_state_machine();
 
 	// Publish FTS controller state
-	_fts_controller_state.timestamp = hrt_absolute_time();
+	_fts_state.timestamp = hrt_absolute_time();
-	_fts_controller_state.fts_comm_timeout = _fts_state.fts_comm_timeout;
+	_fts_state.afts_comm_timeout = 0;
-	_fts_controller_state.fts_state = _fts_state.fts_state;
+	_fts_state.main_fts_state = 0;
-	_fts_controller_state_pub.publish(_fts_controller_state);
+	//_fts_state_pub.publish(_fts_state);
 
 	perf_end(_cycle_perf);
 }
@@ -74,7 +71,6 @@ void FTSController::update_status()
 {
 
 	// Update FTS state
-	_fts_state.fts_state = _fts_status_sub.get().main_state;
 
 	// Update GROUND-FTS communication state
 	_fts_state.gfts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us;
@@ -83,7 +79,7 @@ void FTSController::update_status()
 
 }
 
-void FTSController::send_fts_status()
+void FTSController::send_fts_state()
 {
 }
 
@@ -91,8 +87,8 @@ void FTSController::send_fts_status()
 void
 FTSController::update_topics()
 {
-	if (_fts_status_sub.update()) {
+	if (_fts_state_sub.update()) {
-		_last_fts_state_update_time = _fts_status_sub.get().timestamp;
+		_last_fts_state_update_time = _fts_state_sub.get().timestamp;
 	}
 
 }
@@ -176,7 +172,7 @@ FTSController::print_status()
 	perf_print_counter(_interval_perf);
 
 	PX4_INFO("Current state machine state:%u", _main_state_machine.get_mainstatemachine_state());
-	PX4_INFO("Current FTS state:%u", _fts_state.fts_state);
+	PX4_INFO("Current FTS state:%u", _fts_state.main_fts_state);
 
 	return 0;
 }

src/modules/fts_control/fts_controller.hpp

@@ -14,13 +14,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/fts_state.h>
-#include <uORB/topics/fts_server_cmd_ack.h>
-#include <uORB/topics/fts_control_cmd.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/fts_server_cmd.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/fts_controller_state.h>
 
 #include "main_state_machine.hpp"
 
@@ -49,35 +45,26 @@ public:
 private:
 
 	void Run() override;
-	void update_server_cmd();
+	void send_fts_state();
 	void update_topics();
 	void update_status();
 	bool update_param();
+    void fire();
+    void reset();
 
 	perf_counter_t _cycle_perf;
 	perf_counter_t _interval_perf;
 
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::SubscriptionData<fts_state_s> _fts_status_sub{ORB_ID(fts_state)};
+	uORB::SubscriptionData<fts_state_s> _fts_state_sub{ORB_ID(fts_state)};
-	uORB::SubscriptionData<fts_server_cmd_s> _fts_server_cmd_sub{ORB_ID(fts_server_cmd)};
 	uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_local_position_s> _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
 
-	uORB::Publication<fts_server_cmd_ack_s> _fts_ground_cmd_reply_pub{ORB_ID(fts_server_cmd_ack)};
-	uORB::Publication<fts_control_cmd_s> _fts_control_cmd_pub{ORB_ID(fts_control_cmd)};
-	uORB::Publication<fts_controller_state_s> _fts_controller_state_pub{ORB_ID(fts_controller_state)};
 
-	fts_controller_state_s _fts_controller_state{0};
 	hrt_abstime _last_fts_state_update_time{0};
 
 	MainStateMachine _main_state_machine;
 	MainStateMachine::fts_state_s _fts_state{};
 
-	bool _new_server_cmd_arrive{false};
-
-	float _minimum_batt_capacity{.5f};
-	float _minimum_vehicle_altitude_before_close_box{10.0f};
-
 	uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz
 };

src/modules/fts_control/main_state_machine.cpp

@@ -0,0 +1,63 @@
+#include "main_state_machine.hpp"
+
+MainStateMachine::MainStateMachine()
+{
+}
+
+uint8_t MainStateMachine::get_mainstatemachine_state()
+{
+    return 0;
+}
+
+void MainStateMachine::init_state_machine(uORB::Publication<fts_state_s> *fts_state_pub,
+		MainStateMachine::fts_state_s *FTS_state)
+{
+}
+
+void MainStateMachine::send_cmd_ack(uint8_t cmd_result)
+{
+
+	//ground_fts_reply_s msg{};
+	//msg.timestamp = hrt_absolute_time();
+	//msg.cmd_result = cmd_result;
+
+	//_ground_cmd_reply_pub->publish(msg);
+}
+
+bool MainStateMachine::send_fts_cmd(uint8_t cmd)
+{
+
+
+
+	return true;
+}
+
+bool MainStateMachine::check_ground_fts_timeout()
+{
+	if (hrt_elapsed_time(&_last_mfts_ping_time) > FTS_PING_TIMEOUT) {
+		return true;
+	}
+	return false;
+}
+bool MainStateMachine::check_auto_fts_timeout()
+{
+    if (hrt_elapsed_time(&_last_afts_ping_time) > FTS_PING_TIMEOUT) {
+        return true;
+    }
+    return false;
+}
+
+
+void MainStateMachine::run_state_machine()
+{
+	switch (0) {
+	case 0:
+		break;
+
+	case 1:
+		break;
+
+	case 2:
+		break;
+	}
+}

src/modules/fts_control/main_state_machine.hpp

@@ -1,37 +1,39 @@
-//
-// Created by tianfang on 06/05/22.
-//
 
 #ifndef PX4_MAIN_STATE_MACHINE_HPP
 #define PX4_MAIN_STATE_MACHINE_HPP
 
+#define FTS_PING_TIMEOUT 1000
+#include <px4_platform_common/module.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/fts_state.h>
-#include <uORB/topics/fts_control_cmd.h>
 #include <uORB/Publication.hpp>
 
 using namespace time_literals;
 
-class MainStateMachine : public StateMachineHelper<MainStateMachine>
+class MainStateMachine : public ModuleBase<MainStateMachine>
 {
 
 public:
 
 	struct fts_state_s {
+        uint64_t timestamp{0};
 		bool gfts_comm_timeout{false};
         bool afts_comm_timeout{false};
-		uint8_t ground_fts_state{ground_fts_state_s::MAIN_STATE_UNKNOWN};
+		uint8_t ground_fts_state{0};
-        uint8_t auto_fts_state{auto_fts_state_s::MAIN_STATE_UNKNOWN};
+        uint8_t auto_fts_state{0};
-        uint8_t main_fts_state{main_fts_state_s::MAIN_STATE_IDLE};
+        uint8_t main_fts_state{0};
 
     };
 
 	MainStateMachine();
-	void init_state_machine(uORB::Publication<main_fts_state_s> *main_fts_state_pub,
+	void init_state_machine(uORB::Publication<fts_state_s> *fts_state_pub,
 				MainStateMachine::fts_state_s *FTS_state);
-	void run_state_machine() override;
+	void run_state_machine();
 	uint8_t get_mainstatemachine_state();
-
+    void send_cmd_ack(uint8_t cmd_result);
+    bool send_fts_cmd(uint8_t cmd);
+    bool check_ground_fts_timeout();
+    bool check_auto_fts_timeout();
 private:
 
 	hrt_abstime _last_mfts_ping_time{0};

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit 18955a04c7c7467e00ea42b704addb4a9c12b53a
+Subproject commit 4474134e1f9f2f8bc228b6751268294ee3aa84ed