Feature/update serial init

ROMFS/px4fmu_common/init.d/rcS

@@ -602,13 +602,6 @@ else
 		zenoh start
 	fi
 
-	#Check if the Vertiq serial driver has somewhere to go
-	if param greater -s VERTIQ_IO_CFG 0
-	then
-		vertiq_io start
-	fi
-
-
 #
 # End of autostart.
 #

src/drivers/actuators/vertiq_io/module.yaml

@@ -1,6 +1,6 @@
 module_name: Vertiq IO
 serial_config:
-    - command: vertiq_io iquart_port ${SERIAL_DEV}
+    - command: vertiq_io start -d ${SERIAL_DEV}
       port_config_param:
         name: VERTIQ_IO_CFG
         group: Vertiq IO

src/drivers/actuators/vertiq_io/vertiq_io.cpp

@@ -35,9 +35,8 @@
 #include <px4_platform_common/log.h>
 
 px4::atomic_bool VertiqIo::_request_telemetry_init{false};
-char VertiqIo::_telemetry_device[] {};
 
-VertiqIo::VertiqIo() :
+VertiqIo::VertiqIo(const char *port) :
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
 	_serial_interface(),
 	_client_manager(&_serial_interface),
@@ -47,6 +46,12 @@ VertiqIo::VertiqIo() :
 	_broadcast_arming_handler(_kBroadcastID),
 	_operational_ifci(_kBroadcastID)
 {
+	// store port name
+	strncpy(_port, port, sizeof(_port) - 1);
+
+	// enforce null termination
+	_port[sizeof(_port) - 1] = '\0';
+
 	//Make sure we get the correct initial values for our parameters
 	updateParams();
 
@@ -60,6 +65,9 @@ VertiqIo::VertiqIo() :
 
 VertiqIo::~VertiqIo()
 {
+	//Be inactive
+	stop();
+
 	//Free our counters/timers
 	perf_free(_loop_perf);
 	perf_free(_loop_interval_perf);
@@ -68,6 +76,8 @@ VertiqIo::~VertiqIo()
 //called by our task_spawn function
 bool VertiqIo::init()
 {
+	_serial_interface.InitSerial(_port, _param_vertiq_baud.get());
+
 #ifdef CONFIG_USE_IFCI_CONFIGURATION
 	//Grab the number of IFCI control values the user wants to use
 	_cvs_in_use = (uint8_t)_param_vertiq_number_of_cvs.get();
@@ -90,6 +100,17 @@ bool VertiqIo::init()
 	return true;
 }
 
+void VertiqIo::start()
+{
+	ScheduleNow();
+}
+
+void VertiqIo::stop()
+{
+	ScheduleClear();
+}
+
+
 //This is the same as a while(1) loop
 void VertiqIo::Run()
 {
@@ -98,19 +119,6 @@ void VertiqIo::Run()
 	perf_begin(_loop_perf);
 	perf_count(_loop_interval_perf);
 
-	//If we should leave, then clean up our mess and get out
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	//Someone asked to change the telemetry port, or we booted up
-	if (_request_telemetry_init.load()) {
-		_serial_interface.InitSerial(_telemetry_device, _param_vertiq_baud.get());
-		_request_telemetry_init.store(false);
-	}
-
 	//Handle IQUART reception and transmission
 	_client_manager.HandleClientCommunication();
 
@@ -143,7 +151,6 @@ void VertiqIo::Run()
 	perf_end(_loop_perf);
 }
 
-
 void VertiqIo::parameters_update()
 {
 	//If someone has changed any parameter in our module. Checked at 1Hz
@@ -243,75 +250,142 @@ bool VertiqIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	return true;
 }
 
-int VertiqIo::task_spawn(int argc, char *argv[])
+void VertiqIo::print_info()
 {
-	VertiqIo *instance = new VertiqIo();
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
 
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
+	_mixing_output.printStatus();
+}
 
-		if (instance->init()) {
-			return PX4_OK;
-		}
+/**
+ * Local functions in support of the shell command.
+ */
+namespace vertiq_namespace
+{
 
-	} else {
-		PX4_ERR("alloc failed");
+VertiqIo	*g_dev{nullptr};
+
+int start(const char *port);
+int status();
+int stop();
+int usage();
+
+int
+start(const char *port)
+{
+	if (g_dev != nullptr) {
+		PX4_ERR("already started");
+		return PX4_OK;
+	}
+
+	// Instantiate the driver.
+	g_dev = new VertiqIo(port);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver start failed");
+		return PX4_ERROR;
 	}
 
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
+	if (!g_dev->init()) {
+		PX4_ERR("driver start failed");
+		delete g_dev;
+		g_dev = nullptr;
+		return PX4_ERROR;
+	}
 
-	return PX4_ERROR;
+	return PX4_OK;
 }
 
-int VertiqIo::print_status()
+int
+status()
 {
-	perf_print_counter(_loop_perf);
-	perf_print_counter(_loop_interval_perf);
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
 
-	_mixing_output.printStatus();
 	return 0;
 }
 
-int VertiqIo::print_usage(const char *reason)
+int stop()
 {
-	if (reason) {
-		PX4_WARN("%s\n", reason);
+	if (g_dev != nullptr) {
+		PX4_INFO("stopping driver");
+		delete g_dev;
+		g_dev = nullptr;
+		PX4_INFO("driver stopped");
+
+	} else {
+		PX4_ERR("driver not running");
+		return 1;
 	}
 
+	return PX4_OK;
+}
+
+int
+usage()
+{
 	PRINT_MODULE_USAGE_NAME("vertiq_io", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-
-	PRINT_MODULE_USAGE_COMMAND_DESCR("iquart_port", "Enable IQUART on a UART port.");
 	PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);
-
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
+	return PX4_OK;
 }
 
-int VertiqIo::custom_command(int argc, char *argv[])
+} // namespace
+
+extern "C" __EXPORT int vertiq_io_main(int argc, char *argv[])
 {
-	const char *verb = argv[0];
-
-	if (!strcmp(verb, "iquart_port")) {
-		if (argc > 1) {
-			// telemetry can be requested before the module is started
-			strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1);
-			_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
-			_request_telemetry_init.store(true);
+	int ch = 0;
+	const char *device_path = nullptr;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+
+		case 'd':
+			device_path = myoptarg;
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+			return PX4_ERROR;
 		}
+	}
 
-		return 0;
+	if (myoptind >= argc) {
+		PX4_ERR("unrecognized command");
+		return vertiq_namespace::usage();
 	}
 
-	return print_usage("unknown command");
-}
+	if (!device_path) {
+		PX4_ERR("Missing device");
+		return PX4_ERROR;
+	}
 
+	if (!strcmp(argv[myoptind], "start")) {
+		if (strcmp(device_path, "") != 0) {
+			return vertiq_namespace::start(device_path);
 
-extern "C" __EXPORT int vertiq_io_main(int argc, char *argv[])
-{
-	return VertiqIo::main(argc, argv);
+		} else {
+			PX4_WARN("Please specify device path!");
+			return vertiq_namespace::usage();
+		}
+
+	} else if (!strcmp(argv[myoptind], "stop")) {
+		return vertiq_namespace::stop();
+
+	} else if (!strcmp(argv[myoptind], "status")) {
+		return vertiq_namespace::status();
+	}
+
+	return vertiq_namespace::usage();
 }

src/drivers/actuators/vertiq_io/vertiq_io.hpp

@@ -39,6 +39,7 @@
 
 #include <px4_log.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/getopt.h>
 
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -67,38 +68,30 @@
 enum DISARM_BEHAVIORS {TRIGGER_MOTOR_DISARM, COAST_MOTOR, SEND_PREDEFINED_VELOCITY};
 enum ARM_BEHAVIORS {USE_MOTOR_ARMING, FORCE_ARMING};
 
-class VertiqIo : public ModuleBase<VertiqIo>, public OutputModuleInterface
+class VertiqIo : public OutputModuleInterface
 {
 
 public:
 	/**
 	* @brief Create a new VertiqIo object
 	*/
-	VertiqIo();
+	VertiqIo(const char *port);
 
 	/**
 	* @brief destruct a VertiqIo object
 	*/
-	~VertiqIo() override;
+	~VertiqIo();
 
 	/**
 	* @brief initialize the VertiqIo object. This will be called by the task_spawn function. Makes sure that the thread gets scheduled.
 	*/
 	bool init();
 
-	/** @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);
-
-	int print_status() override;
-
-	/** @see ModuleBase::run() */
-	void Run() override;
+	/**
+	 * @brief Print information about how to use our module
+	 *
+	 */
+	void print_info();
 
 	/** @see OutputModuleInterface */
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -116,6 +109,11 @@ class VertiqIo : public ModuleBase<VertiqIo>, public OutputModuleInterface
 	 * Check for parameter changes and update them if needed.
 	 */
 	void parameters_update();
+	void Run() override;
+	void start();
+	void stop();
+
+	char _port[20] {};
 
 	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")};