fts module implementation start

.gitmodules

@@ -62,6 +62,3 @@
 	path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client
 	url = https://github.com/PX4/Micro-XRCE-DDS-Client.git
 	branch = px4
-[submodule "src/modules/mavlink/mavlink_nimbus"]
-	path = src/modules/mavlink/mavlink_nimbus
-	url = git@bitbucket.org:nimbussrl/mavlinknimbusc.git

boards/px4/fts-proto/src/board_config.h

@@ -149,6 +149,11 @@
 #define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
 #define V6C21   HW_VER_REV(0x2,0x1) // FMUV6CMini,             Rev 1  I2C4 Internal I2C2 External
 
+/*FTS*/
+#define GPIO_FTS_CAN_OUTPUT   /* PB9  FTS CAN CUT */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_FTS_CHUTE_OUTPUT   /* PB9  FTS CHUTE FIRE */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define FTS_TERMINATE_CAN(on_true)	       px4_arch_gpiowrite(GPIO_FTS_CAN_OUTPUT, (on_true))
+#define FTS_TRIGGER_CHUTE(on_true)	       px4_arch_gpiowrite(GPIO_FTS_CHUTE_OUTPUT, (on_true))
 
 /* HEATER
  * PWM in future
@@ -266,6 +271,8 @@
 		GPIO_VDD_5V_HIPOWER_nOC,          \
 		GPIO_VDD_3V3_SENSORS_EN,          \
 		GPIO_TONE_ALARM_IDLE,             \
+		GPIO_FTS_CAN_OUTPUT,               \
+		GPIO_FTS_CHUTE_OUTPUT,               \
 	}
 
 #define BOARD_ENABLE_CONSOLE_BUFFER

build_afts.sh

@@ -5,4 +5,5 @@ git submodule update --recursive
 #make px4_fmu-v6x_default
 #make px4_fmu-v6c_default
 #make px4_fmu-v6c_bootloader
-make -j${nproc} px4_fts-can
+#make -j$(nproc) px4_fts-can
+make px4_fts-can

build_mfts.sh

@@ -3,5 +3,6 @@
 
 rm -rf build/
 git submodule update --recursive
-#make -j${nproc} px4_fts-can
-make -j${nproc} px4_fts-proto
+#make -j$(nproc) px4_fts-can
+#make -j$(nproc) px4_fts-proto
+make px4_fts-proto

msg/CMakeLists.txt

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

msg/FTSStatus.msg

@@ -0,0 +1,5 @@
+uint64 timestamp	# time since system start (microseconds)
+
+bool fts_on
+
+

src/drivers/fts/CMakeLists.txt

@@ -0,0 +1,7 @@
+px4_add_module(
+	MODULE drivers__fts
+	MAIN fts
+	COMPILE_FLAGS
+	SRCS
+		fts.cpp
+	)

src/drivers/fts/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_FTS
+	bool "fts"
+	default y
+	---help---
+		Enable support for fts

src/drivers/fts/fts.cpp

@@ -0,0 +1,236 @@
+/**
+ * @file fts.cpp
+ */
+
+#include "fts.h"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <drivers/drv_hrt.h>
+
+#define IO_HEATER_DEVICE_PATH "/dev/px4io"
+
+#define _IO_HEATER_BASE       (0x2e00)
+
+#define PX4IO_HEATER_CONTROL  _PX4_IOC(_IO_HEATER_BASE, 0)
+
+/* ... to IOX_SET_VALUE + 8 */
+
+/* enum passed to IO_HEATER_CONTROL ioctl()*/
+enum IO_HEATER_MODE {
+    HEATER_MODE_OFF = 0,
+    HEATER_MODE_ON = 1,
+    HEATER_MODE_DISABLED = -1
+};
+
+// Use direct calls to turn GPIO pin on/off
+#  if not defined(GPIO_FTS_CAN_OUTPUT) || if not defined(GPIO_FTS_CHUTE_OUTPUT)
+#  error "To use the fts driver, the board_config.h must define and initialize GPIO_FTS_CAN_OUTPUT and GPIO_FTS_CHUTE_OUTPUT "
+#  endif
+
+FTS::FTS() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+
+	_fts_status_pub.advertise();
+}
+
+FTS::~FTS()
+{
+	disable_fts();
+}
+
+int FTS::custom_command(int argc, char *argv[])
+{
+	// Check if the driver is running.
+	if (!is_running()) {
+		PX4_INFO("not running");
+		return PX4_ERROR;
+	}
+
+	return print_usage("Unrecognized command.");
+}
+
+void FTS::disable_fts()
+{
+	// Reset fts to off state.
+    px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT);
+    px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT);
+}
+
+void FTS::initialize_fts_io()
+{
+	// Initialize fts to off state.
+
+px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT);
+px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT);
+}
+
+void FTS::fts_off()
+{
+    FTS_TERMINATE_CAN(false);
+}
+
+void FTS::fts_on()
+{
+    FTS_TERMINATE_CAN(true);
+}
+
+bool FTS::initialize_topics()
+{
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
+
+		if (sensor_accel_sub.get().timestamp != 0 &&
+		    sensor_accel_sub.get().device_id != 0 &&
+		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
+
+			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
+			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
+				_sensor_accel_sub.ChangeInstance(i);
+				_sensor_device_id = sensor_accel_sub.get().device_id;
+				initialize_fts_io();
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void FTS::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	update_params();
+
+	if (_sensor_device_id == 0) {
+		if (!initialize_topics()) {
+			// if sensor still not found try again in 1 second
+			ScheduleDelayed(1_s);
+			return;
+		}
+	}
+
+	sensor_accel_s sensor_accel;
+	float temperature_delta {0.f};
+
+	if (_fts_on) {
+		// Turn the fts off.
+		_fts_on = false;
+		fts_off();
+		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
+
+	} else if (_sensor_accel_sub.update(&sensor_accel)) {
+
+		// Update the current IMU sensor temperature if valid.
+		if (PX4_ISFINITE(sensor_accel.temperature)) {
+			temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature;
+			_temperature_last = sensor_accel.temperature;
+		}
+
+		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
+		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
+
+		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
+
+		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
+					   _integrator_value) * static_cast<float>(_controller_period_usec));
+
+		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
+
+		if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
+			_temperature_target_met = true;
+
+		} else {
+
+			_temperature_target_met = false;
+		}
+
+		_fts_on = true;
+		fts_on();
+		ScheduleDelayed(_controller_time_on_usec);
+	}
+
+	publish_status();
+}
+
+void FTS::publish_status()
+{
+	fts_status_s status{};
+	status.fts_on               = _fts_on;
+	status.timestamp = hrt_absolute_time();
+	_fts_status_pub.publish(status);
+}
+
+int FTS::start()
+{
+	// Exit the driver if the sensor ID does not match the desired sensor.
+	if (_param_sens_temp_id.get() == 0) {
+		PX4_ERR("Valid SENS_TEMP_ID required");
+		request_stop();
+		return PX4_ERROR;
+	}
+
+	update_params(true);
+	ScheduleNow();
+	return PX4_OK;
+}
+
+int FTS::task_spawn(int argc, char *argv[])
+{
+	FTS *fts = new FTS();
+
+	if (!fts) {
+		PX4_ERR("driver allocation failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(fts);
+	_task_id = task_id_is_work_queue;
+
+	fts->start();
+	return 0;
+}
+
+void FTS::update_params(const bool force)
+{
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+	}
+}
+
+int FTS::print_usage(const char *reason)
+{
+	if (reason) {
+		printf("%s\n\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
+
+This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fts", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fts_main(int argc, char *argv[])
+{
+	return FTS::main(argc, argv);
+}

src/drivers/fts/fts.h

@@ -0,0 +1,100 @@
+/**
+ * @file fts.h
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#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/parameter_update.h>
+
+#include <mathlib/mathlib.h>
+
+using namespace time_literals;
+
+class FTS : public ModuleBase<FTS>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	FTS();
+
+	virtual ~FTS();
+
+	/**
+	 * @see ModuleBase::custom_command().
+	 * @brief main Main entry point to the module that should be
+	 *        called directly from the module's main method.
+	 * @param argc The input argument count.
+	 * @param argv Pointer to the input argument array.
+	 * @return Returns 0 iff successful, -1 otherwise.
+	 */
+	static int custom_command(int argc, char *argv[]);
+
+	/**
+	 * @see ModuleBase::print_usage().
+	 * @brief Prints the module usage to the nuttshell console.
+	 * @param reason The requested reason for printing to console.
+	 */
+	static int print_usage(const char *reason = nullptr);
+
+	/**
+	 * @see ModuleBase::task_spawn().
+	 * @brief Initializes the class in the same context as the work queue
+	 *        and starts the background listener.
+	 * @param argv Pointer to the input argument array.
+	 * @return Returns 0 iff successful, -1 otherwise.
+	 */
+	static int task_spawn(int argc, char *argv[]);
+
+	/**
+	 * @brief Initiates the fts driver work queue, starts a new background task,
+	 *        and fails if it is already running.
+	 * @return Returns 1 iff start was successful.
+	 */
+	int start();
+
+private:
+
+	/** Disables the fts */
+	void disable_fts();
+
+	/** Turns the fts on */
+	void fts_on();
+
+	/** Turns the fts off */
+	void fts_off();
+
+	void initialize();
+
+	/** Enables / configures the fts GPIO */
+	void initialize_fts_io();
+
+	/** @brief Called once to initialize uORB topics. */
+	bool initialize_topics();
+
+	void publish_status();
+
+	/**
+	 * @brief Updates and checks for updated uORB parameters.
+	 * @param force Boolean to determine if an update check should be forced.
+	 */
+	void update_params(const bool force = false);
+
+	/** Work queue struct for the scheduler. */
+	static struct work_s _work;
+
+	/** File descriptor for PX4IO for fts ioctl's */
+
+	bool _fts_initialized     = false;
+	bool _fts_on              = false;
+
+	uORB::Publication<fts_status_s> _fts_status_pub{ORB_ID(fts_status)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+};

src/drivers/fts/fts_params.c

@@ -0,0 +1,5 @@
+/**
+ * @file fts_params.c
+ * FTS parameters.
+ */
+

src/modules/fts/CMakeLists.txt

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

src/modules/fts/fts_controller.cpp

@@ -0,0 +1,211 @@
+/**
+ *
+ * Controller of Nimbus Charging FTS
+ *
+ * @author Flavio Tonelli <f.tonelli@nimbus.aero>
+ */
+
+#include "fts_controller.hpp"
+
+FTSController::FTSController() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem("fts_controller", px4::wq_configurations::lp_default),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "fts_controller"": cycle")),
+	_interval_perf(perf_alloc(PC_INTERVAL, "fts_controller"": interval"))
+{
+}
+
+FTSController::~FTSController()
+{
+	ScheduleClear();
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+}
+
+bool
+FTSController::init()
+{
+	// Update parameters
+	if (!update_param()) {
+		PX4_WARN("fts_controller 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);
+	return true;
+}
+
+void
+FTSController::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
+
+	// Update all subscriber
+	update_topics();
+
+	// 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_controller_state.fts_comm_timeout = _fts_state.fts_comm_timeout;
+	_fts_controller_state.fts_state = _fts_state.fts_state;
+	_fts_controller_state_pub.publish(_fts_controller_state);
+
+	perf_end(_cycle_perf);
+}
+
+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;
+    // Update AUTO-FTS communication state
+    _fts_state.afts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us;
+
+}
+
+void FTSController::send_fts_status()
+{
+}
+
+
+void
+FTSController::update_topics()
+{
+	if (_fts_status_sub.update()) {
+		_last_fts_state_update_time = _fts_status_sub.get().timestamp;
+	}
+
+}
+
+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_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;
+
+	return true;
+}
+
+int
+FTSController::task_spawn(int argc, char *argv[])
+{
+	FTSController *instance = new FTSController();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+void
+FTSController::fire()
+{
+    px4_arch_gpiowrite(GPIO_OUTPUT|5, true);
+}
+
+void
+FTSController::reset()
+{
+    px4_arch_gpiowrite(GPIO_OUTPUT|5, false);
+}
+
+int
+FTSController::custom_command(int argc, char *argv[])
+{
+    if (!strcmp(argv[0], "fire")) {
+        get_instance()->fire();
+        return 0;
+    }
+
+    if (!strcmp(argv[0], "reset")) {
+        get_instance()->reset();
+        return 0;
+    }
+
+    return print_usage("unknown command");
+}
+
+int
+FTSController::print_status()
+{
+	perf_print_counter(_cycle_perf);
+	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);
+
+	return 0;
+}
+
+int
+FTSController::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fts_controller", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND("status");
+    PRINT_MODULE_USAGE_COMMAND("fire");
+    PRINT_MODULE_USAGE_COMMAND("reset");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fts_controller_main(int argc, char *argv[])
+{
+	return FTSController::main(argc, argv);
+}

src/modules/fts/fts_controller.hpp

@@ -0,0 +1,83 @@
+/**
+ *
+ * Controller of Nimbus Charging FTS
+ *
+ * @author Flavio Tonelli <f.tonelli@nimbus.aero>
+ */
+
+#pragma once
+
+#include <lib/parameters/param.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#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"
+
+using namespace time_literals;
+
+class FTSController : public ModuleBase<FTSController>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	FTSController();
+
+	~FTSController() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	int print_status() override;
+
+private:
+
+	void Run() override;
+	void update_server_cmd();
+	void update_topics();
+	void update_status();
+	bool update_param();
+
+	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_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/fts_controller_params.c

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fts_controller_params.c
+ * Parameters for FTS controller.
+ *
+ * @author Germano Girardi <germano.girardi@neosystems.it>
+ */
+
+/**
+ * Enable FTS relates funtions.
+ *
+ * @boolean
+ * @group FTS Control
+ */
+PARAM_DEFINE_INT32(FTS_EN, 0);
+
+/**
+ * The timeout for fts_state message
+ *
+ * @unit s
+ * @reboot_required true
+ * @decimal 1
+ *
+ * @group FTS Control
+ */
+PARAM_DEFINE_FLOAT(FTS_COMM_TM, 10.0f);

src/modules/fts/main_state_machine.cpp

@@ -0,0 +1,89 @@
+#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/main_state_machine.hpp

@@ -0,0 +1,45 @@
+//
+// Created by tianfang on 06/05/22.
+//
+
+#ifndef PX4_MAIN_STATE_MACHINE_HPP
+#define PX4_MAIN_STATE_MACHINE_HPP
+
+#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>
+{
+
+public:
+
+	struct fts_state_s {
+		bool gfts_comm_timeout{false};
+        bool afts_comm_timeout{false};
+		uint8_t ground_fts_state{ground_fts_state_s::MAIN_STATE_UNKNOWN};
+        uint8_t auto_fts_state{auto_fts_state_s::MAIN_STATE_UNKNOWN};
+        uint8_t main_fts_state{main_fts_state_s::MAIN_STATE_IDLE};
+
+    };
+
+	MainStateMachine();
+	void init_state_machine(uORB::Publication<main_fts_state_s> *main_fts_state_pub,
+				MainStateMachine::fts_state_s *FTS_state);
+	void run_state_machine() override;
+	uint8_t get_mainstatemachine_state();
+
+private:
+
+	hrt_abstime _last_mfts_ping_time{0};
+    hrt_abstime _last_afts_ping_time{0};
+    uORB::Publication<fts_state_s> *_fts_state_pub{nullptr};
+
+	static constexpr uint64_t FTS_STATE_SEND_INTERVAL = 200_ms; // Limit the publication of the FTS state at 5 Hz
+};
+
+
+#endif //PX4_MAIN_STATE_MACHINE_HPP

src/modules/mavlink/CMakeLists.txt

@@ -33,6 +33,8 @@
 
 set(MAVLINK_GIT_DIR "${CMAKE_CURRENT_LIST_DIR}/mavlink")
 set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink")
+#set(MAVLINK_GIT_DIR "${CMAKE_CURRENT_LIST_DIR}/mavlink_nimbus")
+#set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink_nimbus")
 file(RELATIVE_PATH MAVLINK_GIT_DIR_RELATIVE ${CMAKE_SOURCE_DIR} ${MAVLINK_GIT_DIR})
 
 px4_add_git_submodule(TARGET git_mavlink_v2 PATH "${MAVLINK_GIT_DIR}")

src/modules/mavlink/mavlink_nimbus

@@ -1 +0,0 @@
-Subproject commit 71ae8c334f1602db04249216e4d1a5c628889420